Merge pull request #489 from erwincoumans/master

Implement CMD_INIT_POSE to set base position, base orientation and jo…

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -259,6 +259,70 @@ int	b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan
     return 0;
 }
 
+b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
+{
+	PhysicsClient* cl = (PhysicsClient* ) physClient;
+    b3Assert(cl);
+    b3Assert(cl->canSubmitCommand());
+    struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
+    b3Assert(command);
+    command->m_type = CMD_INIT_POSE;
+    command->m_updateFlags =0;
+	command->m_initPoseArgs.m_bodyUniqueId = bodyIndex;
+    return (b3SharedMemoryCommandHandle) command;
+}
+
+int	b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_INIT_POSE);
+    command->m_updateFlags |=INIT_POSE_HAS_INITIAL_POSITION;
+	command->m_initPoseArgs.m_initialStateQ[0] = startPosX;
+	command->m_initPoseArgs.m_initialStateQ[1] = startPosY;
+	command->m_initPoseArgs.m_initialStateQ[2] = startPosZ;
+	return 0;
+}
+
+int	b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_INIT_POSE);
+    command->m_updateFlags |=INIT_POSE_HAS_INITIAL_ORIENTATION;
+	command->m_initPoseArgs.m_initialStateQ[3] = startOrnX;
+	command->m_initPoseArgs.m_initialStateQ[4] = startOrnY;
+	command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ;
+	command->m_initPoseArgs.m_initialStateQ[6] = startOrnW;
+	return 0;
+}
+
+int	b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, double* jointPositions)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_INIT_POSE);
+    command->m_updateFlags |=INIT_POSE_HAS_JOINT_STATE;
+	for (int i=0;i<numJointPositions;i++)
+	{
+		command->m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i];
+	}
+	return 0;
+}
+
+int	b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition)
+{
+	struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
+    b3Assert(command);
+    b3Assert(command->m_type == CMD_INIT_POSE);
+    command->m_updateFlags |=INIT_POSE_HAS_JOINT_STATE;
+	b3JointInfo info;
+	b3GetJointInfo(physClient, command->m_initPoseArgs.m_bodyUniqueId,jointIndex, &info);
+	command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition;
+	return 0;
+}
+
+
 int	b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ)
 {
     struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

examples/SharedMemory/PhysicsClientC_API.h

@@ -85,6 +85,13 @@ int	b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle
 int	b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
 int	b3CreateBoxCommandSetHalfExtents(b3SharedMemoryCommandHandle commandHandle, double halfExtentsX,double halfExtentsY,double halfExtentsZ);
 
+///Initialize (teleport) the pose of a body/robot. You can individually set the base position, base orientation and joint angles.
+///This will set all velocities of base and joints to zero.
+b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
+int	b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
+int	b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
+int	b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, double* jointPositions);
+int	b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle,  int jointIndex, double jointPosition);
 
 b3SharedMemoryCommandHandle b3CreateSensorCommandInit(b3PhysicsClientHandle physClient);
 int b3CreateSensorEnable6DofJointForceTorqueSensor(b3SharedMemoryCommandHandle commandHandle, int jointIndex, int enable);

examples/SharedMemory/PhysicsClientExample.cpp

@@ -116,7 +116,7 @@ public:
 
 		
 			
-			float color[4] = {1,1,0,1};
+			float color[4] = {0.2,0.2,1,1};
 			
 			if (points.size() && indices.size())
 			{
@@ -221,6 +221,58 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
             break;
         };
 
+		case CMD_INIT_POSE:
+		{
+			
+			if (m_selectedBody>=0)
+			{
+				b3SharedMemoryCommandHandle commandHandle = b3CreatePoseCommandInit(m_physicsClientHandle,m_selectedBody);
+				static int toggle = 0;
+				double pos[3] = {0,0,0};
+				pos[toggle] = 2;
+				toggle++; 
+				if (toggle>2)
+					toggle=0;
+
+				btQuaternion orn;
+				orn.setValue(0,0,0,1);
+
+				switch (toggle)
+				{
+				case 0:
+					orn = btQuaternion(btVector3(1,0,0),SIMD_HALF_PI);
+					break;
+				case 1:
+					orn = btQuaternion(btVector3(0,1,0),SIMD_HALF_PI);
+					break;
+				case 2:
+					orn = btQuaternion(btVector3(0,0,1),SIMD_HALF_PI);
+					break;
+
+				default:
+					orn.setValue(0,0,0,1);
+				};
+				
+
+				b3CreatePoseCommandSetBaseOrientation(commandHandle,orn[0],orn[1],orn[2],orn[3]);
+				b3CreatePoseCommandSetBasePosition(commandHandle, pos[0],pos[1],pos[2]);
+				int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
+				static double jointPos = SIMD_PI/2.f;
+				
+				for (int i=0;i<numJoints;i++)
+				{
+					b3JointInfo info;
+					b3GetJointInfo(m_physicsClientHandle, m_selectedBody,i, &info);
+					if ((info.m_jointType == 0) || (info.m_jointType == 1)) //revolute or prismatic
+					{
+						b3CreatePoseCommandSetJointPosition(m_physicsClientHandle,commandHandle,i,jointPos);
+					}
+				}
+				jointPos += SIMD_PI/8.0;
+				b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
+			}
+			break;
+		}
         case CMD_STEP_FORWARD_SIMULATION:
         {
         
@@ -309,6 +361,7 @@ void	PhysicsClientExample::createButtons()
         createButton("Send Desired State",CMD_SEND_DESIRED_STATE,  isTrigger);
         createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
         createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
+		createButton("Initialize Pose",CMD_INIT_POSE,  isTrigger);
 
 
 		if (m_selectedBody>=0)
@@ -383,47 +436,53 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
 		createButtons();
 		m_prevSelectedBody = m_selectedBody;
 	}
-    b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
-    bool hasStatus = (status != 0);
     
-	if (hasStatus)
+	//while (!b3CanSubmitCommand(m_physicsClientHandle))
-    {
+	{
-
+		b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle);
-        int statusType = b3GetStatusType(status);
+		bool hasStatus = (status != 0);
-        
+		if (hasStatus)
-      	if (statusType == CMD_URDF_LOADING_COMPLETED)
 		{
-			int bodyIndex = b3GetStatusBodyIndex(status);
-			if (bodyIndex>=0)
-			{
-				int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
 
-				for (int i=0;i<numJoints;i++)
+			int statusType = b3GetStatusType(status);
+			if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+			{
+				//b3Printf("bla\n");
+			}
+      		if (statusType == CMD_URDF_LOADING_COMPLETED)
+			{
+				int bodyIndex = b3GetStatusBodyIndex(status);
+				if (bodyIndex>=0)
 				{
-					b3JointInfo info;
+					int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
-					b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
+            
-					b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
+					for (int i=0;i<numJoints;i++)
+					{
+						b3JointInfo info;
+						b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
+						b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
+				
+					}
+					ComboBoxParams comboParams;
+					comboParams.m_comboboxId = bodyIndex;
+					comboParams.m_numItems = 1;
+					comboParams.m_startItem = 0;
+					comboParams.m_callback = MyComboBoxCallback;
+					comboParams.m_userPointer = this;
+					const char* bla = "bla";
+					const char* blarray[1];
+					blarray[0] = bla;
+				
+					comboParams.m_items=blarray;//{&bla};
+					m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
+		
 
 				}
-				ComboBoxParams comboParams;
-				comboParams.m_comboboxId = bodyIndex;
-				comboParams.m_numItems = 1;
-				comboParams.m_startItem = 0;
-				comboParams.m_callback = MyComboBoxCallback;
-				comboParams.m_userPointer = this;
-				const char* bla = "bla";
-				const char* blarray[1];
-				blarray[0] = bla;
-				
-				comboParams.m_items=blarray;//{&bla};
-				m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
-		
 
 			}
     
 		}
-    
+	}
-    }
     if (b3CanSubmitCommand(m_physicsClientHandle))
     {
         if (m_userCommandRequests.size())
@@ -447,6 +506,7 @@ void	PhysicsClientExample::stepSimulation(float deltaTime)
                 createButtons();
             }
 			
+            
             prepareAndSubmitCommand(commandId);
             
         }  else

examples/SharedMemory/PhysicsClientSharedMemory.cpp

@@ -235,8 +235,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                                                      mb->m_links[link].m_jointName);
                                         }
                                         info.m_jointName = mb->m_links[link].m_jointName;
-                                        info.m_jointType = mb->m_links[link].m_jointType;
                                     }
+
+									info.m_jointType = mb->m_links[link].m_jointType;
+
                                     if ((mb->m_links[link].m_jointType == eRevoluteType) ||
                                         (mb->m_links[link].m_jointType == ePrismaticType)) {
                                         info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
@@ -278,8 +280,8 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
                                                      mb->m_links[link].m_jointName);
                                         }
                                         info.m_jointName = mb->m_links[link].m_jointName;
-                                        info.m_jointType = mb->m_links[link].m_jointType;
                                     }
+									info.m_jointType = mb->m_links[link].m_jointType;
                                     if ((mb->m_links[link].m_jointType == eRevoluteType) ||
                                         (mb->m_links[link].m_jointType == ePrismaticType)) {
                                         info.m_flags |= JOINT_HAS_MOTORIZED_POWER;

examples/SharedMemory/PhysicsServer.cpp

@@ -1252,15 +1252,38 @@ void PhysicsServerSharedMemory::processClientCommands()
 					if (body && body->m_multiBody)
 					{
 						btMultiBody* mb = body->m_multiBody;
-						mb->setBasePos(btVector3(
+						if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION)
-								clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[0],
+						{
-								clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[1],
+							btVector3 zero(0,0,0);
-								clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[2]));
+							mb->setBaseVel(zero);
-						mb->setWorldToBaseRot(btQuaternion(
+							mb->setBasePos(btVector3(
-								clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[3],
+									clientCmd.m_initPoseArgs.m_initialStateQ[0],
-								clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[4],
+									clientCmd.m_initPoseArgs.m_initialStateQ[1],
-								clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[5],
+									clientCmd.m_initPoseArgs.m_initialStateQ[2]));
-								clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[6]));
+						}
+						if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION)
+						{
+							mb->setBaseOmega(btVector3(0,0,0));
+							mb->setWorldToBaseRot(btQuaternion(
+									clientCmd.m_initPoseArgs.m_initialStateQ[3],
+									clientCmd.m_initPoseArgs.m_initialStateQ[4],
+									clientCmd.m_initPoseArgs.m_initialStateQ[5],
+									clientCmd.m_initPoseArgs.m_initialStateQ[6]));
+						}
+						if (clientCmd.m_updateFlags & INIT_POSE_HAS_JOINT_STATE)
+						{
+							int dofIndex = 7;
+							for (int i=0;i<mb->getNumLinks();i++)
+							{
+								if (mb->getLink(i).m_dofCount==1)
+								{
+									
+									mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]);
+									mb->setJointVel(i,0);
+								}
+								dofIndex += mb->getLink(i).m_dofCount;
+							}
+						}
 					}
 					
 					SharedMemoryStatus& serverCmd =m_data->createServerStatus(CMD_CLIENT_COMMAND_COMPLETED,clientCmd.m_sequenceNumber,timeStamp);

examples/SharedMemory/SharedMemoryCommands.h

@@ -65,6 +65,12 @@ struct SetJointFeedbackArgs
 	int m_isEnabled;
 };
 
+enum EnumInitPoseFlags
+{
+    INIT_POSE_HAS_INITIAL_POSITION=1,
+    INIT_POSE_HAS_INITIAL_ORIENTATION=2,
+    INIT_POSE_HAS_JOINT_STATE=4
+};
 
 
 ///InitPoseArgs is mainly to initialize (teleport) the robot in a particular position