void PhysicsClientExample::createButtons() { bool isTrigger = false; if (m_guiHelper && m_guiHelper->getParameterInterface()) { m_guiHelper->getParameterInterface()->removeAllParameters(); createButton("Load URDF",CMD_LOAD_URDF, isTrigger); createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger); createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger); createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger); createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger); createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger); createButton("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger); createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger); createButton("Initialize Pose",CMD_INIT_POSE, isTrigger); createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger); if (m_physicsClientHandle && m_selectedBody>=0) { int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody); for (int i=0;i<numJoints;i++) { b3JointInfo info; b3GetJointInfo(m_physicsClientHandle,m_selectedBody,i,&info); b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { if (m_numMotors<MAX_NUM_MOTORS) { char motorName[1024]; sprintf(motorName,"%s q", info.m_jointName); // MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors]; MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors]; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; motorInfo->m_qIndex = info.m_qIndex; // SliderParams slider(motorName,&motorInfo->m_velTarget); // slider.m_minVal=-4; // slider.m_maxVal=4; SliderParams slider(motorName,&motorInfo->m_posTarget); slider.m_minVal=-4; slider.m_maxVal=4; if (m_guiHelper && m_guiHelper->getParameterInterface()) { m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } m_numMotors++; } } } } } }
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; }
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state) { const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; b3Assert(status); int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId; b3Assert(bodyIndex>=0); if (bodyIndex>=0) { b3JointInfo info; b3GetJointInfo(physClient, bodyIndex,jointIndex, &info); state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex]; state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex]; for (int ii(0); ii < 6; ++ii) { state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii]; } } }
int main(int argc, char* argv[]) { kPhysClient = b3CreateInProcessPhysicsServerAndConnect(argc, argv); if (!kPhysClient) return -1; // visualizer command = b3InitConfigureOpenGLVisualizer(kPhysClient); b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_GUI, 0); b3SubmitClientCommandAndWaitStatus(kPhysClient, command); b3ConfigureOpenGLVisualizerSetVisualizationFlags(command, COV_ENABLE_SHADOWS, 0); b3SubmitClientCommandAndWaitStatus(kPhysClient, command); b3SetTimeOut(kPhysClient, 10); //syncBodies is only needed when connecting to an existing physics server that has already some bodies command = b3InitSyncBodyInfoCommand(kPhysClient); statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); statusType = b3GetStatusType(statusHandle); // set fixed time step command = b3InitPhysicsParamCommand(kPhysClient); ret = b3PhysicsParamSetTimeStep(command, FIXED_TIMESTEP); statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); ret = b3PhysicsParamSetRealTimeSimulation(command, false); statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); // load test command = b3LoadUrdfCommandInit(kPhysClient, "TwoJointRobot_wo_fixedJoints.urdf"); int flags = URDF_USE_INERTIA_FROM_FILE; b3LoadUrdfCommandSetFlags(command, flags); b3LoadUrdfCommandSetUseFixedBase(command, true); // q.setEulerZYX(0, 0, 0); // b3LoadUrdfCommandSetStartOrientation(command, q[0], q[1], q[2], q[3]); b3LoadUrdfCommandSetUseMultiBody(command, true); statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); statusType = b3GetStatusType(statusHandle); b3Assert(statusType == CMD_URDF_LOADING_COMPLETED); if (statusType == CMD_URDF_LOADING_COMPLETED) { twojoint = b3GetStatusBodyIndex(statusHandle); } //disable default linear/angular damping b3SharedMemoryCommandHandle command = b3InitChangeDynamicsInfo(kPhysClient); double linearDamping = 0; double angularDamping = 0; b3ChangeDynamicsInfoSetLinearDamping(command, twojoint, linearDamping); b3ChangeDynamicsInfoSetAngularDamping(command, twojoint, angularDamping); statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); int numJoints = b3GetNumJoints(kPhysClient, twojoint); printf("twojoint numjoints = %d\n", numJoints); // Loop through all joints for (int i = 0; i < numJoints; ++i) { b3GetJointInfo(kPhysClient, twojoint, i, &jointInfo); if (jointInfo.m_jointName[0]) { jointNameToId[std::string(jointInfo.m_jointName)] = i; } else { continue; } // Reset before torque control - see #1459 command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_VELOCITY); b3JointControlSetDesiredVelocity(command, jointInfo.m_uIndex, 0); b3JointControlSetMaximumForce(command, jointInfo.m_uIndex, 0); statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); } // loop unsigned long dtus1 = (unsigned long)1000000.0 * FIXED_TIMESTEP; double simTimeS = 0; double q[2], v[2]; while (b3CanSubmitCommand(kPhysClient)) { simTimeS += 0.000001 * dtus1; // apply some torque b3GetJointInfo(kPhysClient, twojoint, jointNameToId["joint_2"], &jointInfo); command = b3JointControlCommandInit2(kPhysClient, twojoint, CONTROL_MODE_TORQUE); b3JointControlSetDesiredForceTorque(command, jointInfo.m_uIndex, 0.5 * sin(10 * simTimeS)); statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); // get joint values command = b3RequestActualStateCommandInit(kPhysClient, twojoint); statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, command); b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_1"], &state); q[0] = state.m_jointPosition; v[0] = state.m_jointVelocity; b3GetJointState(kPhysClient, statusHandle, jointNameToId["joint_2"], &state); q[1] = state.m_jointPosition; v[1] = state.m_jointVelocity; statusHandle = b3SubmitClientCommandAndWaitStatus(kPhysClient, b3InitStepSimulationCommand(kPhysClient)); // debugging output printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", simTimeS, q[0], q[1], v[0], v[1]); b3Clock::usleep(1000. * 1000. * FIXED_TIMESTEP); } b3DisconnectSharedMemory(kPhysClient); return 0; }
int main(int argc, char* argv[]) { int i, dofCount , posVarCount, dofIndex, ret ,numJoints, allowSharedMemoryInitialization=0; int sensorJointIndexLeft=-1; int sensorJointIndexRight=-1; int statusType = -1; const char* urdfFileName = "r2d2.urdf"; double gravx=0, gravy=0, gravz=-9.8; double timeStep = 1./60.; double startPosX, startPosY,startPosZ; int imuLinkIndex = -1; b3PhysicsClientHandle sm=0; int bodyIndex = -1; printf("hello world\n"); #ifdef PHYSICS_LOOP_BACK sm = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY); #else sm = b3ConnectSharedMemory(SHARED_MEMORY_KEY); #endif if (b3CanSubmitCommand(sm)) { { b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); ret = b3PhysicsParamSetGravity(command, gravx,gravy, gravz); ret = b3PhysicsParamSetTimeStep(command, timeStep); b3SubmitClientCommandAndWaitStatus(sm, command); } { b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); //setting the initial position, orientation and other arguments are optional startPosX =2; startPosY =3; startPosZ = 1; ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); bodyIndex = b3GetStatusBodyIndex(statusHandle); } if (bodyIndex>=0) { numJoints = b3GetNumJoints(sm,bodyIndex); for (i=0;i<numJoints;i++) { struct b3JointInfo jointInfo; b3GetJointInfo(sm,bodyIndex, i,&jointInfo); printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName); //pick the IMU link index based on torso name if (strstr(jointInfo.m_linkName,"base_link")) { imuLinkIndex = i; } //pick the joint index based on joint name if (strstr(jointInfo.m_jointName,"base_to_left_leg")) { sensorJointIndexLeft = i; } if (strstr(jointInfo.m_jointName,"base_to_right_leg")) { sensorJointIndexRight = i; } } if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0)) { b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm); b3SharedMemoryStatusHandle statusHandle; if (imuLinkIndex>=0) { ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1); } if (sensorJointIndexLeft>=0) { ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1); } if(sensorJointIndexRight>=0) { ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } } { b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm); ret = b3CreateBoxCommandSetStartPosition(command, 0,0,-1); ret = b3CreateBoxCommandSetStartOrientation(command,0,0,0,1); ret = b3CreateBoxCommandSetHalfExtents(command, 10,10,1); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } { int statusType; b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm,bodyIndex); b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { b3GetStatusActualState(statusHandle, 0, &posVarCount, &dofCount, 0, 0, 0, 0); b3Printf("posVarCount = %d\n",posVarCount); printf("dofCount = %d\n",dofCount); } } { #if 0 b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY); for ( dofIndex=0;dofIndex<dofCount;dofIndex++) { b3JointControlSetDesiredVelocity(command,dofIndex,1); b3JointControlSetMaximumForce(command,dofIndex,100); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); #endif } ///perform some simulation steps for testing for ( i=0;i<100;i++) { b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm)); } { b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex)); if (sensorJointIndexLeft>=0) { struct b3JointSensorState sensorState; b3GetJointState(sm,state,sensorJointIndexLeft,&sensorState); b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft, sensorState.m_jointForceTorque[0], sensorState.m_jointForceTorque[1], sensorState.m_jointForceTorque[2]); } if (sensorJointIndexRight>=0) { struct b3JointSensorState sensorState; b3GetJointState(sm,state,sensorJointIndexRight,&sensorState); b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, sensorState.m_jointForceTorque[0], sensorState.m_jointForceTorque[1], sensorState.m_jointForceTorque[2]); } } { b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm)); } } b3DisconnectSharedMemory(sm); }
B3_SHARED_API int preTickPluginCallback_vrSyncPlugin(struct b3PluginContext* context) { MyClass* obj = (MyClass*)context->m_userPointer; if (obj && obj->m_controllerId >= 0) { { int i = 0; { for (int n = 0; n < context->m_numVRControllerEvents; n++) { const b3VRControllerEvent& event = context->m_vrControllerEvents[n]; if (event.m_controllerId == obj->m_controllerId) { if (obj->m_constraintId >= 0) { struct b3UserConstraint constraintInfo; if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId, &constraintInfo)) { //this is basically equivalent to doing this in Python/pybullet: //p.changeConstraint(pr2_cid, e[POSITION], e[ORIENTATION], maxForce=...) b3SharedMemoryCommandHandle commandHandle; int userConstraintUniqueId = obj->m_constraintId; commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, userConstraintUniqueId); double pos[4] = {event.m_pos[0], event.m_pos[1], event.m_pos[2], 1}; b3InitChangeUserConstraintSetPivotInB(commandHandle, pos); double orn[4] = {event.m_orn[0], event.m_orn[1], event.m_orn[2], event.m_orn[3]}; b3InitChangeUserConstraintSetFrameInB(commandHandle, orn); b3InitChangeUserConstraintSetMaxForce(commandHandle, obj->m_maxForce); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); } } // apply the analogue button to close the constraint, using a gear constraint with position target if (obj->m_constraintId2 >= 0) { struct b3UserConstraint constraintInfo; if (b3GetUserConstraintInfo(context->m_physClient, obj->m_constraintId2, &constraintInfo)) { //this block is similar to //p.changeConstraint(c,gearRatio=1, erp=..., relativePositionTarget=relPosTarget, maxForce=...) //printf("obj->m_constraintId2=%d\n", obj->m_constraintId2); b3SharedMemoryCommandHandle commandHandle; commandHandle = b3InitChangeUserConstraintCommand(context->m_physClient, obj->m_constraintId2); //0 -> open, 1 = closed double openPos = 1.; double relPosTarget = openPos - (event.m_analogAxis * openPos); b3InitChangeUserConstraintSetRelativePositionTarget(commandHandle, relPosTarget); b3InitChangeUserConstraintSetERP(commandHandle, 1); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, commandHandle); } } //printf("event.m_analogAxis=%f\n", event.m_analogAxis); // use the pr2_gripper motors to keep the gripper centered/symmetric around the center axis if (obj->m_gripperId >= 0) { //this block is similar to //b = p.getJointState(pr2_gripper,2)[0] //print("b = " + str(b)) //p.setJointMotorControl2(pr2_gripper, 0, p.POSITION_CONTROL, targetPosition=b, force=3) //printf("obj->m_gripperId=%d\n", obj->m_gripperId); { b3SharedMemoryCommandHandle cmd_handle = b3RequestActualStateCommandInit(context->m_physClient, obj->m_gripperId); b3SharedMemoryStatusHandle status_handle = b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle); int status_type = b3GetStatusType(status_handle); if (status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { //printf("status_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED\n"); b3JointSensorState sensorState; if (b3GetJointState(context->m_physClient, status_handle, 2, &sensorState)) { b3SharedMemoryCommandHandle commandHandle; double targetPosition = sensorState.m_jointPosition; //printf("targetPosition =%f\n", targetPosition); if (1) { b3JointInfo info; b3GetJointInfo(context->m_physClient, obj->m_gripperId, 0, &info); commandHandle = b3JointControlCommandInit2(context->m_physClient, obj->m_gripperId, CONTROL_MODE_POSITION_VELOCITY_PD); double kp = .1; double targetVelocity = 0; double kd = .6; b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); b3JointControlSetKp(commandHandle, info.m_uIndex, kp); b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); b3JointControlSetKd(commandHandle, info.m_uIndex, kd); b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, obj->m_maxForce2); b3SubmitClientCommandAndWaitStatus(context->m_physClient, cmd_handle); } } else { //printf("???\n"); } } else { //printf("no\n"); } } } } } } } } return 0; }
void testSharedMemory(b3PhysicsClientHandle sm) { int i, dofCount , posVarCount, ret ,numJoints ; int sensorJointIndexLeft=-1; int sensorJointIndexRight=-1; const char* urdfFileName = "r2d2.urdf"; const char* sdfFileName = "kuka_iiwa/model.sdf"; double gravx=0, gravy=0, gravz=-9.8; double timeStep = 1./60.; double startPosX, startPosY,startPosZ; int imuLinkIndex = -1; int bodyIndex = -1; if (b3CanSubmitCommand(sm)) { { b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm); b3SharedMemoryStatusHandle statusHandle; ret = b3PhysicsParamSetGravity(command, gravx,gravy, gravz); ret = b3PhysicsParamSetTimeStep(command, timeStep); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); } { b3SharedMemoryStatusHandle statusHandle; int statusType; int bodyIndicesOut[10];//MAX_SDF_BODIES = 10 int numJoints, numBodies; int bodyUniqueId; b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(sm, sdfFileName); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); ASSERT_EQ(statusType, CMD_SDF_LOADING_COMPLETED); numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, 10); ASSERT_EQ(numBodies,1); bodyUniqueId = bodyIndicesOut[0]; { { b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3InitRequestVisualShapeInformation(sm, bodyUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_VISUAL_SHAPE_INFO_COMPLETED) { struct b3VisualShapeInformation vi; b3GetVisualShapeInformation(sm, &vi); } } } numJoints = b3GetNumJoints(sm,bodyUniqueId); ASSERT_EQ(numJoints,7); #if 0 b3Printf("numJoints: %d\n", numJoints); for (i=0;i<numJoints;i++) { struct b3JointInfo jointInfo; if (b3GetJointInfo(sm,bodyUniqueId, i,&jointInfo)) { b3Printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName); } } #endif { b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryCommandHandle commandHandle; double jointAngle = 0.f; int jointIndex; commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId); for (jointIndex=0;jointIndex<numJoints;jointIndex++) { b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, jointAngle); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); } } { b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(sm, urdfFileName); //setting the initial position, orientation and other arguments are optional startPosX =2; startPosY =0; startPosZ = 1; ret = b3LoadUrdfCommandSetStartPosition(command, startPosX,startPosY,startPosZ); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); ASSERT_EQ(statusType, CMD_URDF_LOADING_COMPLETED); bodyIndex = b3GetStatusBodyIndex(statusHandle); } if (bodyIndex>=0) { numJoints = b3GetNumJoints(sm,bodyIndex); for (i=0;i<numJoints;i++) { struct b3JointInfo jointInfo; b3GetJointInfo(sm,bodyIndex, i,&jointInfo); // printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName); //pick the IMU link index based on torso name if (strstr(jointInfo.m_linkName,"base_link")) { imuLinkIndex = i; } //pick the joint index based on joint name if (strstr(jointInfo.m_jointName,"base_to_left_leg")) { sensorJointIndexLeft = i; } if (strstr(jointInfo.m_jointName,"base_to_right_leg")) { sensorJointIndexRight = i; } } if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0)) { b3SharedMemoryCommandHandle command = b3CreateSensorCommandInit(sm, bodyIndex); b3SharedMemoryStatusHandle statusHandle; if (imuLinkIndex>=0) { ret = b3CreateSensorEnableIMUForLink(command, imuLinkIndex, 1); } if (sensorJointIndexLeft>=0) { ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexLeft, 1); } if(sensorJointIndexRight>=0) { ret = b3CreateSensorEnable6DofJointForceTorqueSensor(command, sensorJointIndexRight, 1); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED); } } { b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryCommandHandle command = b3CreateBoxShapeCommandInit(sm); ret = b3CreateBoxCommandSetStartPosition(command, 0,0,-1); ret = b3CreateBoxCommandSetStartOrientation(command,0,0,0,1); ret = b3CreateBoxCommandSetHalfExtents(command, 10,10,1); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RIGID_BODY_CREATION_COMPLETED); } { int statusType; b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(sm,bodyIndex); b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); ASSERT_EQ(statusType, CMD_ACTUAL_STATE_UPDATE_COMPLETED); if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { b3GetStatusActualState(statusHandle, 0, &posVarCount, &dofCount, 0, 0, 0, 0); ASSERT_EQ(posVarCount,15); ASSERT_EQ(dofCount,14); } } { #if 0 b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryCommandHandle command = b3JointControlCommandInit( sm, CONTROL_MODE_VELOCITY); for ( dofIndex=0;dofIndex<dofCount;dofIndex++) { b3JointControlSetDesiredVelocity(command,dofIndex,1); b3JointControlSetMaximumForce(command,dofIndex,100); } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); #endif } ///perform some simulation steps for testing for ( i=0;i<100;i++) { b3SharedMemoryStatusHandle statusHandle; int statusType; if (b3CanSubmitCommand(sm)) { statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitStepSimulationCommand(sm)); statusType = b3GetStatusType(statusHandle); ASSERT_EQ(statusType, CMD_STEP_FORWARD_SIMULATION_COMPLETED); } else { break; } } if (b3CanSubmitCommand(sm)) { b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex)); if (sensorJointIndexLeft>=0) { struct b3JointSensorState sensorState; b3GetJointState(sm,state,sensorJointIndexLeft,&sensorState); b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft, sensorState.m_jointForceTorque[0], sensorState.m_jointForceTorque[1], sensorState.m_jointForceTorque[2]); } if (sensorJointIndexRight>=0) { struct b3JointSensorState sensorState; b3GetJointState(sm,state,sensorJointIndexRight,&sensorState); b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, sensorState.m_jointForceTorque[0], sensorState.m_jointForceTorque[1], sensorState.m_jointForceTorque[2]); } { b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(sm, b3InitResetSimulationCommand(sm)); ASSERT_EQ(b3GetStatusType(statusHandle), CMD_RESET_SIMULATION_COMPLETED); } } } b3DisconnectSharedMemory(sm); }
void PhysicsClientExample::stepSimulation(float deltaTime) { if (m_options == eCLIENTEXAMPLE_SERVER) { for (int i=0;i<100;i++) { m_physicsServer.processClientCommands(); } } if (m_prevSelectedBody != m_selectedBody) { createButtons(); m_prevSelectedBody = m_selectedBody; } //while (!b3CanSubmitCommand(m_physicsClientHandle)) { b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle); bool hasStatus = (status != 0); if (hasStatus) { int statusType = b3GetStatusType(status); if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { //b3Printf("bla\n"); } if (statusType ==CMD_CAMERA_IMAGE_COMPLETED) { static int counter=0; char msg[1024]; sprintf(msg,"Camera image %d OK\n",counter++); b3CameraImageData imageData; b3GetCameraImageData(m_physicsClientHandle,&imageData); if (m_canvas && m_canvasIndex >=0) { for (int i=0;i<imageData.m_pixelWidth;i++) { for (int j=0;j<imageData.m_pixelHeight;j++) { int xIndex = int(float(i)*(float(camVisualizerWidth)/float(imageData.m_pixelWidth))); int yIndex = int(float(j)*(float(camVisualizerHeight)/float(imageData.m_pixelHeight))); btClamp(yIndex,0,imageData.m_pixelHeight); btClamp(xIndex,0,imageData.m_pixelWidth); int bytesPerPixel = 4; int pixelIndex = (i+j*imageData.m_pixelWidth)*bytesPerPixel; m_canvas->setPixel(m_canvasIndex,xIndex,camVisualizerHeight-1-yIndex, imageData.m_rgbColorData[pixelIndex], imageData.m_rgbColorData[pixelIndex+1], imageData.m_rgbColorData[pixelIndex+2], 255); // imageData.m_rgbColorData[pixelIndex+3]); } } m_canvas->refreshImageData(m_canvasIndex); } b3Printf(msg); } if (statusType == CMD_CAMERA_IMAGE_FAILED) { b3Printf("Camera image FAILED\n"); } 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++) { 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); } } } } if (b3CanSubmitCommand(m_physicsClientHandle)) { if (m_userCommandRequests.size()) { //b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); int commandId = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;i<m_userCommandRequests.size();i++) { m_userCommandRequests[i-1] = m_userCommandRequests[i]; } m_userCommandRequests.pop_back(); //for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders if (commandId ==CMD_RESET_SIMULATION) { m_selectedBody = -1; m_numMotors=0; createButtons(); b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle); if (m_options == eCLIENTEXAMPLE_SERVER) { b3SubmitClientCommand(m_physicsClientHandle, commandHandle); while (!b3CanSubmitCommand(m_physicsClientHandle)) { m_physicsServer.processClientCommands(); b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle); bool hasStatus = (status != 0); if (hasStatus) { int statusType = b3GetStatusType(status); b3Printf("Status after reset: %d",statusType); } } } else { prepareAndSubmitCommand(commandId); } } else { prepareAndSubmitCommand(commandId); } } else { if (m_numMotors) { enqueueCommand(CMD_SEND_DESIRED_STATE); enqueueCommand(CMD_STEP_FORWARD_SIMULATION); if (m_options != eCLIENTEXAMPLE_SERVER) { enqueueCommand(CMD_REQUEST_DEBUG_LINES); } //enqueueCommand(CMD_REQUEST_ACTUAL_STATE); } } } }
void PhysicsClientExample::prepareAndSubmitCommand(int commandId) { switch (commandId) { case CMD_LOAD_URDF: { b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf"); //setting the initial position, orientation and other arguments are optional double startPosX = 0; static double startPosY = 0; double startPosZ = 0; b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX,startPosY,startPosZ); startPosY += 2.f; // ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_CAMERA_IMAGE_DATA: { ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle); float viewMatrix[16]; float projectionMatrix[16]; this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); this->m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix); b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix,projectionMatrix); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CREATE_BOX_COLLISION_SHAPE: { b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); b3CreateBoxCommandSetStartPosition(commandHandle,0,0,-3); b3CreateBoxCommandSetColorRGBA(commandHandle,0,0,1,1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CREATE_RIGID_BODY: { b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); b3CreateBoxCommandSetStartPosition(commandHandle,0,0,0); b3CreateBoxCommandSetMass(commandHandle,1); b3CreateBoxCommandSetCollisionShapeType(commandHandle,COLLISION_SHAPE_TYPE_CYLINDER_Y); b3CreateBoxCommandSetColorRGBA(commandHandle,1,1,0,1); double radius = 0.2; double halfHeight = 0.5; b3CreateBoxCommandSetHalfExtents(commandHandle,radius,halfHeight,radius); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_ACTUAL_STATE: { if (m_selectedBody>=0) { b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle,m_selectedBody); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody); for (int i = 0; i < numJoints; ++i) { struct b3JointSensorState sensorState; b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState); b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque); } } 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: { b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_DEBUG_LINES: { b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_SEND_DESIRED_STATE: { // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_VELOCITY); b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, CONTROL_MODE_POSITION_VELOCITY_PD); prepareControlCommand(command); b3SubmitClientCommand(m_physicsClientHandle, command); break; } case CMD_RESET_SIMULATION: { b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_SEND_BULLET_DATA_STREAM: { #if 0 //this worked, but needs C-API and a streaming options, similar to debug lines command.m_type = buttonId; cl->enqueueCommand(command); #endif break; } case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle); b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } default: { b3Error("Unknown buttonId"); btAssert(0); } }; }
void PhysicsClientExample::stepSimulation(float deltaTime) { if (m_prevSelectedBody != m_selectedBody) { createButtons(); m_prevSelectedBody = m_selectedBody; } //while (!b3CanSubmitCommand(m_physicsClientHandle)) { b3SharedMemoryStatusHandle status = b3ProcessServerStatus(m_physicsClientHandle); bool hasStatus = (status != 0); if (hasStatus) { 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) { int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex); 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); } } } } if (b3CanSubmitCommand(m_physicsClientHandle)) { if (m_userCommandRequests.size()) { //b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size()); int commandId = m_userCommandRequests[0]; //a manual 'pop_front', we don't use 'remove' because it will re-order the commands for (int i=1;i<m_userCommandRequests.size();i++) { m_userCommandRequests[i-1] = m_userCommandRequests[i]; } m_userCommandRequests.pop_back(); //for the CMD_RESET_SIMULATION we need to do something special: clear the GUI sliders if (commandId ==CMD_RESET_SIMULATION) { m_selectedBody = -1; m_numMotors=0; createButtons(); } prepareAndSubmitCommand(commandId); } else { if (m_numMotors) { enqueueCommand(CMD_SEND_DESIRED_STATE); enqueueCommand(CMD_STEP_FORWARD_SIMULATION); enqueueCommand(CMD_REQUEST_DEBUG_LINES); //enqueueCommand(CMD_REQUEST_ACTUAL_STATE); } } } }
void PhysicsClientExample::createButtons() { bool isTrigger = false; if (m_guiHelper && m_guiHelper->getParameterInterface()) { m_guiHelper->getParameterInterface()->removeAllParameters(); createButton("Load URDF", CMD_LOAD_URDF, isTrigger); createButton("Load SDF", CMD_LOAD_SDF, isTrigger); createButton("Save World", CMD_SAVE_WORLD, isTrigger); createButton("Set Shadow", CMD_SET_SHADOW, isTrigger); createButton("Update Visual Shape", CMD_UPDATE_VISUAL_SHAPE, isTrigger); createButton("Get Camera Image", CMD_REQUEST_CAMERA_IMAGE_DATA, isTrigger); createButton("Step Sim", CMD_STEP_FORWARD_SIMULATION, isTrigger); createButton("Realtime Sim", CMD_CUSTOM_SET_REALTIME_SIMULATION, isTrigger); createButton("Get Visual Shape Info", CMD_REQUEST_VISUAL_SHAPE_INFO, isTrigger); createButton("Send Bullet Stream", CMD_SEND_BULLET_DATA_STREAM, isTrigger); if (m_options != eCLIENTEXAMPLE_SERVER) { createButton("Get State", CMD_REQUEST_ACTUAL_STATE, isTrigger); } createButton("Send Desired State", CMD_SEND_DESIRED_STATE, isTrigger); createButton("Create Box Collider", CMD_CREATE_BOX_COLLISION_SHAPE, isTrigger); createButton("Create Cylinder Body", CMD_CREATE_RIGID_BODY, isTrigger); createButton("Reset Simulation", CMD_RESET_SIMULATION, isTrigger); createButton("Initialize Pose", CMD_INIT_POSE, isTrigger); createButton("Set gravity", CMD_CUSTOM_SET_GRAVITY, isTrigger); createButton("Compute Inverse Dynamics", CMD_CALCULATE_INVERSE_DYNAMICS, isTrigger); createButton("Get Contact Point Info", CMD_REQUEST_CONTACT_POINT_INFORMATION, isTrigger); if (m_bodyUniqueIds.size()) { if (m_selectedBody < 0) m_selectedBody = 0; ComboBoxParams comboParams; comboParams.m_comboboxId = 0; comboParams.m_numItems = m_bodyUniqueIds.size(); comboParams.m_startItem = m_selectedBody; comboParams.m_callback = MyComboBoxCallback; comboParams.m_userPointer = this; //todo: get the real object name const char** blarray = new const char*[m_bodyUniqueIds.size()]; for (int i = 0; i < m_bodyUniqueIds.size(); i++) { char* bla = new char[16]; sprintf(bla, "%d", i); blarray[i] = bla; comboParams.m_items = blarray; //{&bla}; } m_guiHelper->getParameterInterface()->registerComboBox(comboParams); } if (m_physicsClientHandle && m_selectedBody >= 0) { m_numMotors = 0; int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody); for (int i = 0; i < numJoints; i++) { b3JointInfo info; b3GetJointInfo(m_physicsClientHandle, m_selectedBody, i, &info); //b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex); if (info.m_flags & JOINT_HAS_MOTORIZED_POWER) { if (m_numMotors < MAX_NUM_MOTORS) { char motorName[1024]; sprintf(motorName, "%s q", info.m_jointName); // MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors]; MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors]; motorInfo->m_velTarget = 0.f; motorInfo->m_posTarget = 0.f; motorInfo->m_uIndex = info.m_uIndex; motorInfo->m_qIndex = info.m_qIndex; // SliderParams slider(motorName,&motorInfo->m_velTarget); // slider.m_minVal=-4; // slider.m_maxVal=4; SliderParams slider(motorName, &motorInfo->m_posTarget); slider.m_minVal = -4; slider.m_maxVal = 4; if (m_guiHelper && m_guiHelper->getParameterInterface()) { m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } m_numMotors++; } } } } { SliderParams sliderLightPosX("light source position x", &m_lightPos[0]); SliderParams sliderLightPosY("light source position y", &m_lightPos[1]); SliderParams sliderLightPosZ("light source position z", &m_lightPos[2]); SliderParams sliderSpecularCoeff("specular coefficient", &m_specularCoeff); sliderLightPosX.m_minVal = -1.5; sliderLightPosX.m_maxVal = 1.5; sliderLightPosY.m_minVal = -1.5; sliderLightPosY.m_maxVal = 1.5; sliderLightPosZ.m_minVal = -1.5; sliderLightPosZ.m_maxVal = 1.5; sliderSpecularCoeff.m_minVal = 0; sliderSpecularCoeff.m_maxVal = 5.0; if (m_guiHelper && m_guiHelper->getParameterInterface()) { m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderLightPosX); m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderLightPosY); m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderLightPosZ); m_guiHelper->getParameterInterface()->registerSliderFloatParameter(sliderSpecularCoeff); } } } }
void PhysicsClientExample::prepareAndSubmitCommand(int commandId) { switch (commandId) { case CMD_LOAD_URDF: { b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf"); //setting the initial position, orientation and other arguments are optional double startPosX = 0; static double startPosY = 0; double startPosZ = 0; b3LoadUrdfCommandSetStartPosition(commandHandle, startPosX, startPosY, startPosZ); startPosY += 2.f; // ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_LOAD_SDF: { #ifdef BT_DEBUG b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf"); #else b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "kitchens/1.sdf"); //two_cubes.sdf");//kitchens/1.sdf");//kuka_iiwa/model.sdf"); #endif b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_CAMERA_IMAGE_DATA: { ///request an image from a simulated camera, using a software renderer. b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle); //b3RequestCameraImageSelectRenderer(commandHandle,ER_BULLET_HARDWARE_OPENGL); float viewMatrix[16]; float projectionMatrix[16]; m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix); b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix, projectionMatrix); b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth, camVisualizerHeight); float lightPos[3]; lightPos[0] = m_lightPos[0]; lightPos[1] = m_lightPos[1]; lightPos[2] = m_lightPos[2]; b3RequestCameraImageSetLightDirection(commandHandle, lightPos); b3RequestCameraImageSetLightSpecularCoeff(commandHandle, m_specularCoeff); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CREATE_BOX_COLLISION_SHAPE: { b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); b3CreateBoxCommandSetStartPosition(commandHandle, 0, 0, -1.5); b3CreateBoxCommandSetColorRGBA(commandHandle, 0, 0, 1, 1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CREATE_RIGID_BODY: { b3SharedMemoryCommandHandle commandHandle = b3CreateBoxShapeCommandInit(m_physicsClientHandle); b3CreateBoxCommandSetStartPosition(commandHandle, 0, 0, 0); b3CreateBoxCommandSetMass(commandHandle, 1); b3CreateBoxCommandSetCollisionShapeType(commandHandle, COLLISION_SHAPE_TYPE_CYLINDER_Y); b3CreateBoxCommandSetColorRGBA(commandHandle, 1, 1, 0, 1); double radius = 0.2; double halfHeight = 0.5; b3CreateBoxCommandSetHalfExtents(commandHandle, radius, halfHeight, radius); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_ACTUAL_STATE: { if (m_selectedBody >= 0) { b3SharedMemoryCommandHandle commandHandle = b3RequestActualStateCommandInit(m_physicsClientHandle, m_selectedBody); b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_physicsClientHandle, commandHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody); for (int i = 0; i < numJoints; ++i) { struct b3JointSensorState sensorState; b3GetJointState(m_physicsClientHandle, statusHandle, i, &sensorState); //b3Printf("Joint %d: %f", i, sensorState.m_jointMotorTorque); } } 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: { b3SharedMemoryCommandHandle commandHandle = b3InitStepSimulationCommand(m_physicsClientHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_DEBUG_LINES: { b3SharedMemoryCommandHandle commandHandle = b3InitRequestDebugLinesCommand(m_physicsClientHandle, btIDebugDraw::DBG_DrawWireframe); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_SEND_DESIRED_STATE: { if (m_selectedBody >= 0) { // b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY); b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD); // b3Printf("prepare control command for body %d", m_selectedBody); prepareControlCommand(command); b3SubmitClientCommand(m_physicsClientHandle, command); } break; } case CMD_RESET_SIMULATION: { b3SharedMemoryCommandHandle commandHandle = b3InitResetSimulationCommand(m_physicsClientHandle); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_SEND_BULLET_DATA_STREAM: { #if 0 //this worked, but needs C-API and a streaming options, similar to debug lines command.m_type = buttonId; cl->enqueueCommand(command); #endif break; } case CMD_CUSTOM_SET_GRAVITY: { b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle); b3PhysicsParamSetGravity(commandHandle, 0.0, 0.0, -9.8); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CUSTOM_SET_REALTIME_SIMULATION: { b3SharedMemoryCommandHandle commandHandle = b3InitPhysicsParamCommand(m_physicsClientHandle); b3PhysicsParamSetRealTimeSimulation(commandHandle, 1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_CALCULATE_INVERSE_DYNAMICS: { if (m_selectedBody >= 0) { btAlignedObjectArray<double> jointPositionsQ; btAlignedObjectArray<double> jointVelocitiesQdot; btAlignedObjectArray<double> jointAccelerations; int numJoints = b3GetNumJoints(m_physicsClientHandle, m_selectedBody); if (numJoints) { b3Printf("Compute inverse dynamics for joint accelerations:"); jointPositionsQ.resize(numJoints); jointVelocitiesQdot.resize(numJoints); jointAccelerations.resize(numJoints); for (int i = 0; i < numJoints; i++) { jointAccelerations[i] = 100; b3Printf("Desired joint acceleration[%d]=%f", i, jointAccelerations[i]); } b3SharedMemoryCommandHandle commandHandle = b3CalculateInverseDynamicsCommandInit(m_physicsClientHandle, m_selectedBody, &jointPositionsQ[0], &jointVelocitiesQdot[0], &jointAccelerations[0]); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); } } break; } case CMD_REQUEST_CONTACT_POINT_INFORMATION: { b3SharedMemoryCommandHandle commandHandle = b3InitRequestContactPointInformation(m_physicsClientHandle); b3SetContactFilterBodyA(commandHandle, 0); b3SetContactFilterBodyB(commandHandle, 1); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_SAVE_WORLD: { b3SharedMemoryCommandHandle commandHandle = b3SaveWorldCommandInit(m_physicsClientHandle, "saveWorld.py"); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_REQUEST_VISUAL_SHAPE_INFO: { if (m_selectedBody >= 0) { //request visual shape information b3SharedMemoryCommandHandle commandHandle = b3InitRequestVisualShapeInformation(m_physicsClientHandle, m_selectedBody); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); } break; } case CMD_SET_SHADOW: { b3SharedMemoryCommandHandle commandHandle = b3InitRequestCameraImage(m_physicsClientHandle); float viewMatrix[16]; float projectionMatrix[16]; m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraProjectionMatrix(projectionMatrix); m_guiHelper->getRenderInterface()->getActiveCamera()->getCameraViewMatrix(viewMatrix); b3RequestCameraImageSetCameraMatrices(commandHandle, viewMatrix, projectionMatrix); b3RequestCameraImageSetPixelResolution(commandHandle, camVisualizerWidth, camVisualizerHeight); bool hasShadow = true; b3RequestCameraImageSetShadow(commandHandle, hasShadow); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } case CMD_UPDATE_VISUAL_SHAPE: { int objectUniqueId = 0; int linkIndex = -1; int shapeIndex = -1; int textureIndex = -2; double rgbaColor[4] = {0.0, 1.0, 0.0, 1.0}; b3SharedMemoryCommandHandle commandHandle = b3InitUpdateVisualShape2(m_physicsClientHandle, objectUniqueId, linkIndex, shapeIndex); b3UpdateVisualShapeRGBAColor(commandHandle, rgbaColor); b3SubmitClientCommand(m_physicsClientHandle, commandHandle); break; } default: { b3Error("Unknown buttonId"); btAssert(0); } }; }
int main(int argc, char* argv[]) { int i, dofCount , posVarCount, dofIndex, ret ,numJoints, allowSharedMemoryInitialization=0; int timeout = MAX_TIMEOUT; int sensorJointIndexLeft=-1; int sensorJointIndexRight=-1; const char* urdfFileName = "r2d2.urdf"; double gravx=0, gravy=0, gravz=-9.8; double timeStep = 1./60.; double startPosX, startPosY,startPosZ; int imuLinkIndex = -1; SharedMemoryCommand_t command; SharedMemoryStatus_t status; b3PhysicsClientHandle sm; b3Printf("timeout = %d\n",timeout); printf("hello world\n"); sm = b3ConnectSharedMemory( allowSharedMemoryInitialization); if (b3CanSubmitCommand(sm)) { ret = b3InitPhysicsParamCommand(&command); ret = b3PhysicsParamSetGravity(&command, gravx,gravy, gravz); ret = b3PhysicsParamSetTimeStep(&command, timeStep); ret = b3SubmitClientCommand(sm, &command); timeout = MAX_TIMEOUT; while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} b3Printf("timeout = %d\n",timeout); ret = b3LoadUrdfCommandInit(&command, urdfFileName); //setting the initial position, orientation and other arguments are optional startPosX =2; startPosY =3; startPosZ = 1; ret = b3LoadUrdfCommandSetStartPosition(&command, startPosX,startPosY,startPosZ); ret = b3SubmitClientCommand(sm, &command); timeout = MAX_TIMEOUT; while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} b3Printf("timeout = %d\n",timeout); numJoints = b3GetNumJoints(sm); for (i=0;i<numJoints;i++) { struct b3JointInfo jointInfo; b3GetJointInfo(sm,i,&jointInfo); printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName); //pick the IMU link index based on torso name if (strstr(jointInfo.m_linkName,"base_link")) { imuLinkIndex = i; } //pick the joint index based on joint name if (strstr(jointInfo.m_jointName,"base_to_left_leg")) { sensorJointIndexLeft = i; } if (strstr(jointInfo.m_jointName,"base_to_right_leg")) { sensorJointIndexRight = i; } } if ((sensorJointIndexLeft>=0) || (sensorJointIndexRight>=0)) { ret = b3CreateSensorCommandInit(&command); if (imuLinkIndex>=0) { ret = b3CreateSensorEnableIMUForLink(&command, imuLinkIndex, 1); } if (sensorJointIndexLeft>=0) { ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexLeft, 1); } if(sensorJointIndexRight>=0) { ret = b3CreateSensorEnable6DofJointForceTorqueSensor(&command, sensorJointIndexRight, 1); } ret = b3SubmitClientCommand(sm, &command); timeout = MAX_TIMEOUT; while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} } ret = b3CreateBoxShapeCommandInit(&command); ret = b3CreateBoxCommandSetStartPosition(&command, 0,0,-1); ret = b3CreateBoxCommandSetStartOrientation(&command,0,0,0,1); ret = b3CreateBoxCommandSetHalfExtents(&command, 10,10,1); ret = b3SubmitClientCommand(sm, &command); timeout = MAX_TIMEOUT; while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} b3RequestActualStateCommandInit(&command); ret = b3SubmitClientCommand(sm, &command); timeout = MAX_TIMEOUT; while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ; dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU; b3Printf("posVarCount = %d\n",posVarCount); printf("dofCount = %d\n",dofCount); b3JointControlCommandInit(&command, CONTROL_MODE_VELOCITY); for ( dofIndex=0;dofIndex<dofCount;dofIndex++) { b3JointControlSetDesiredVelocity(&command,dofIndex,1); b3JointControlSetMaximumForce(&command,dofIndex,100); } ret = b3SubmitClientCommand(sm, &command); timeout = MAX_TIMEOUT; while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} ///perform some simulation steps for testing for ( i=0;i<100;i++) { ret = b3InitStepSimulationCommand(&command); ret = b3SubmitClientCommand(sm, &command); timeout = MAX_TIMEOUT; while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} } b3RequestActualStateCommandInit(&command); ret = b3SubmitClientCommand(sm, &command); timeout = MAX_TIMEOUT; while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} if (sensorJointIndexLeft>=0) { b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexLeft, status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+0], status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+1], status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexLeft+2]); } if (sensorJointIndexRight>=0) { b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+0], status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+1], status.m_sendActualStateArgs.m_jointReactionForces[6*sensorJointIndexRight+2]); } ret = b3InitResetSimulationCommand(&command); ret = b3SubmitClientCommand(sm, &command); timeout = MAX_TIMEOUT; while ((timeout-- > 0) && b3ProcessServerStatus(sm, &status)==0) {} } b3DisconnectSharedMemory(sm); }