tweaks in pybullet and shared memory C-API:

allow to reset the state of a single joint
allow to set the target/mode for a single joint motor at a time
rename pybullet API: initializeJointPositions -> resetJointState

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -167,6 +167,10 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle ph
     command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
 	command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId;
 	command->m_updateFlags = 0;
+    for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+    {
+        command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0;
+    }
     return (b3SharedMemoryCommandHandle) command;
 }
 
@@ -176,6 +180,7 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
     b3Assert(command);
     command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
 	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
+    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
 
     return 0;
 }
@@ -186,6 +191,8 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
     b3Assert(command);
     command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
 	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
+    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP;
+
     return 0;
 }
 
@@ -195,6 +202,7 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
     b3Assert(command);
     command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
 	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
+    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD;
 
     return 0;
 }
@@ -205,6 +213,8 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
     b3Assert(command);
     command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
 	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
+    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT;
+
     return 0;
 }
 
@@ -215,6 +225,8 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
     b3Assert(command);
     command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
 	command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+
     return 0;
 }
 
@@ -224,6 +236,8 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
     b3Assert(command);
     command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
     command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+    command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
+
     return 0;
 }
 
@@ -372,6 +386,7 @@ int	b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan
 
 b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
 {
+    
 	PhysicsClient* cl = (PhysicsClient* ) physClient;
     b3Assert(cl);
     b3Assert(cl->canSubmitCommand());
@@ -380,6 +395,11 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl
     command->m_type = CMD_INIT_POSE;
     command->m_updateFlags =0;
 	command->m_initPoseArgs.m_bodyUniqueId = bodyIndex;
+	//a bit slow, initialing the full range to zero...
+	for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
+    {
+        command->m_initPoseArgs.m_hasInitialStateQ[i] = 0;
+    }
     return (b3SharedMemoryCommandHandle) command;
 }
 
@@ -392,6 +412,11 @@ int	b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle
 	command->m_initPoseArgs.m_initialStateQ[0] = startPosX;
 	command->m_initPoseArgs.m_initialStateQ[1] = startPosY;
 	command->m_initPoseArgs.m_initialStateQ[2] = startPosZ;
+	
+	command->m_initPoseArgs.m_hasInitialStateQ[0] = 1;
+	command->m_initPoseArgs.m_hasInitialStateQ[1] = 1;
+	command->m_initPoseArgs.m_hasInitialStateQ[2] = 1;
+	
 	return 0;
 }
 
@@ -405,6 +430,12 @@ int	b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan
 	command->m_initPoseArgs.m_initialStateQ[4] = startOrnY;
 	command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ;
 	command->m_initPoseArgs.m_initialStateQ[6] = startOrnW;
+	
+	command->m_initPoseArgs.m_hasInitialStateQ[3] = 1;
+	command->m_initPoseArgs.m_hasInitialStateQ[4] = 1;
+	command->m_initPoseArgs.m_hasInitialStateQ[5] = 1;
+	command->m_initPoseArgs.m_hasInitialStateQ[6] = 1;
+	
 	return 0;
 }
 
@@ -417,6 +448,7 @@ int	b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand
 	for (int i=0;i<numJointPositions;i++)
 	{
 		command->m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i];
+		command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1;
 	}
 	return 0;
 }
@@ -433,6 +465,7 @@ int	b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
 	if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0)
 	{  
 		command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition;
+		command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex] = 1;
 	}
 	return 0;
 }

examples/SharedMemory/PhysicsClientC_API.h

@@ -99,15 +99,16 @@ b3SharedMemoryCommandHandle	b3LoadSdfCommandInit(b3PhysicsClientHandle physClien
 b3SharedMemoryCommandHandle  b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
 
     
-///Set joint control variables such as desired position/angle, desired velocity,
+///Set joint motor control variables such as desired position/angle, desired velocity,
 ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
 b3SharedMemoryCommandHandle  b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
 
-    ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
+///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
 int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
 int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
 int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
-//Only use when controlMode is CONTROL_MODE_VELOCITY
+
+///Only use when controlMode is CONTROL_MODE_VELOCITY
 int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
 int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex,  double value);
 ///Only use if when controlMode is CONTROL_MODE_TORQUE,

examples/SharedMemory/SharedMemoryCommands.h

@@ -115,6 +115,7 @@ enum EnumInitPoseFlags
 struct InitPoseArgs
 {
 	int m_bodyUniqueId;
+	int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM];
 	double m_initialStateQ[MAX_DEGREE_OF_FREEDOM];
 };
 
@@ -179,11 +180,14 @@ struct SendDesiredStateArgs
 	double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
 	double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
 
+    int m_hasDesiredStateFlags[MAX_DEGREE_OF_FREEDOM];
+    
 	//desired state is only written by the client, read-only access by server is expected
 
 	//m_desiredStateQ is indexed by position variables, 
 	//starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables
     double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
+    
 
 	//m_desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables
     double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];