From a0e507280ca7ad7b6212ae80e7db933df11a687f Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Tue, 13 Oct 2015 22:23:28 -0700 Subject: [PATCH] Implement CMD_INIT_POSE to set base position, base orientation and joint position (angle). See PhysicsClientExample for example use of CMD_INIT_POSE. Change: CMD_INIT_POSE uses m_initPoseArgs and not m_sendDesiredStateCommandArgument --- examples/SharedMemory/PhysicsClientC_API.cpp | 64 +++++++++ examples/SharedMemory/PhysicsClientC_API.h | 9 +- .../SharedMemory/PhysicsClientExample.cpp | 126 +++++++++++++----- .../PhysicsClientSharedMemory.cpp | 6 +- examples/SharedMemory/PhysicsServer.cpp | 41 ++++-- examples/SharedMemory/SharedMemoryCommands.h | 6 + 6 files changed, 207 insertions(+), 45 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index bfa885d61..aed4937b7 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/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;im_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; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index d8b1891db..cc1806bef 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -84,7 +84,14 @@ b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle ph int b3CreateBoxCommandSetStartPosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ); 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); diff --git a/examples/SharedMemory/PhysicsClientExample.cpp b/examples/SharedMemory/PhysicsClientExample.cpp index 4f0996b86..ab444279b 100644 --- a/examples/SharedMemory/PhysicsClientExample.cpp +++ b/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=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) - { - - int statusType = b3GetStatusType(status); - - if (statusType == CMD_URDF_LOADING_COMPLETED) + + //while (!b3CanSubmitCommand(m_physicsClientHandle)) + { + b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle); + bool hasStatus = (status != 0); + if (hasStatus) { - int bodyIndex = b3GetStatusBodyIndex(status); - if (bodyIndex>=0) + + int statusType = b3GetStatusType(status); + if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { - int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex); - - for (int i=0;i=0) { - 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); + int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex); + + for (int i=0;igetParameterInterface()->registerComboBox(comboParams); + comboParams.m_items=blarray;//{&bla}; + m_guiHelper->getParameterInterface()->registerComboBox(comboParams); - } + } - } + } - } + } + } if (b3CanSubmitCommand(m_physicsClientHandle)) { if (m_userCommandRequests.size()) @@ -446,6 +505,7 @@ void PhysicsClientExample::stepSimulation(float deltaTime) m_numMotors=0; createButtons(); } + prepareAndSubmitCommand(commandId); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 20d91a8f9..255bb2bb5 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/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; diff --git a/examples/SharedMemory/PhysicsServer.cpp b/examples/SharedMemory/PhysicsServer.cpp index 73304c3c5..ea924b228 100644 --- a/examples/SharedMemory/PhysicsServer.cpp +++ b/examples/SharedMemory/PhysicsServer.cpp @@ -1252,15 +1252,38 @@ void PhysicsServerSharedMemory::processClientCommands() if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; - mb->setBasePos(btVector3( - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[0], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[1], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[2])); - mb->setWorldToBaseRot(btQuaternion( - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[3], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[4], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[5], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[6])); + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) + { + btVector3 zero(0,0,0); + mb->setBaseVel(zero); + mb->setBasePos(btVector3( + clientCmd.m_initPoseArgs.m_initialStateQ[0], + clientCmd.m_initPoseArgs.m_initialStateQ[1], + clientCmd.m_initPoseArgs.m_initialStateQ[2])); + } + 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;igetNumLinks();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); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 40343d08c..28894199c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/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