class CommonExampleInterface* RobotControlExampleCreateFunc(struct CommonExampleOptions& options) { RobotControlExample* example = new RobotControlExample(options.m_guiHelper, options.m_option); if (gSharedMemoryKey>=0) { example->setSharedMemoryKey(gSharedMemoryKey); } return example; }
void MyCallback2(int buttonId, bool buttonState, void* userPtr) { RobotControlExample* cl = (RobotControlExample*) userPtr; SharedMemoryCommand command; switch (buttonId) { case CMD_LOAD_URDF: { command.m_type =CMD_LOAD_URDF; command.m_updateFlags = URDF_ARGS_FILE_NAME|URDF_ARGS_INITIAL_POSITION|URDF_ARGS_INITIAL_ORIENTATION; sprintf(command.m_urdfArguments.m_urdfFileName,"kuka_lwr/kuka.urdf");//r2d2.urdf"); command.m_urdfArguments.m_initialPosition[0] = 0.0; command.m_urdfArguments.m_initialPosition[1] = 0.0; command.m_urdfArguments.m_initialPosition[2] = 0.0; command.m_urdfArguments.m_initialOrientation[0] = 0.0; command.m_urdfArguments.m_initialOrientation[1] = 0.0; command.m_urdfArguments.m_initialOrientation[2] = 0.0; command.m_urdfArguments.m_initialOrientation[3] = 1.0; command.m_urdfArguments.m_useFixedBase = false; command.m_urdfArguments.m_useMultiBody = true; cl->enqueueCommand(command); break; } case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { //#ifdef USE_C_API b3InitPhysicsParamCommand(&command); b3PhysicsParamSetGravity(&command, 1,1,-10); // #else // // command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS; // command.m_physSimParamArgs.m_gravityAcceleration[0] = 0; // command.m_physSimParamArgs.m_gravityAcceleration[1] = 0; // command.m_physSimParamArgs.m_gravityAcceleration[2] = -10; // command.m_physSimParamArgs.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY; // #endif // USE_C_API cl->enqueueCommand(command); break; }; case CMD_INIT_POSE: { ///@todo: implement this command.m_type = CMD_INIT_POSE; cl->enqueueCommand(command); break; } case CMD_CREATE_BOX_COLLISION_SHAPE: { command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE; command.m_updateFlags = BOX_SHAPE_HAS_INITIAL_POSITION; command.m_createBoxShapeArguments.m_initialPosition[0] = 0; command.m_createBoxShapeArguments.m_initialPosition[1] = 0; command.m_createBoxShapeArguments.m_initialPosition[2] = -3; cl->enqueueCommand(command); break; } case CMD_REQUEST_ACTUAL_STATE: { command.m_type =CMD_REQUEST_ACTUAL_STATE; cl->enqueueCommand(command); break; }; case CMD_STEP_FORWARD_SIMULATION: { command.m_type =CMD_STEP_FORWARD_SIMULATION; cl->enqueueCommand(command); break; } case CMD_SEND_DESIRED_STATE: { command.m_type =CMD_SEND_DESIRED_STATE; cl->prepareControlCommand(command); cl->enqueueCommand(command); break; } case CMD_SEND_BULLET_DATA_STREAM: { command.m_type = buttonId; sprintf(command.m_dataStreamArguments.m_bulletFileName,"slope.bullet"); command.m_dataStreamArguments.m_streamChunkLength = 0; cl->enqueueCommand(command); break; } default: { b3Error("Unknown buttonId"); btAssert(0); } }; }
void MyCallback2(int buttonId, bool buttonState, void* userPtr) { RobotControlExample* cl = (RobotControlExample*) userPtr; SharedMemoryCommand command; switch (buttonId) { case CMD_LOAD_URDF: { command.m_type =CMD_LOAD_URDF; sprintf(command.m_urdfArguments.m_urdfFileName,"r2d2.urdf"); command.m_urdfArguments.m_initialPosition[0] = 0.0; command.m_urdfArguments.m_initialPosition[1] = 0.0; command.m_urdfArguments.m_initialPosition[2] = 0.0; command.m_urdfArguments.m_initialOrientation[0] = 0.0; command.m_urdfArguments.m_initialOrientation[1] = 0.0; command.m_urdfArguments.m_initialOrientation[2] = 0.0; command.m_urdfArguments.m_initialOrientation[3] = 1.0; command.m_urdfArguments.m_useFixedBase = false; command.m_urdfArguments.m_useMultiBody = true; cl->enqueueCommand(command); break; } case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { //#ifdef USE_C_API b3InitPhysicsParamCommand(&command); b3PhysicsParamSetGravity(&command, 0,0,-10); // #else // // command.m_type = CMD_SEND_PHYSICS_SIMULATION_PARAMETERS; // command.m_physSimParamArgs.m_gravityAcceleration[0] = 0; // command.m_physSimParamArgs.m_gravityAcceleration[1] = 0; // command.m_physSimParamArgs.m_gravityAcceleration[2] = -10; // command.m_physSimParamArgs.m_updateFlags = SIM_PARAM_UPDATE_GRAVITY; // #endif // USE_C_API cl->enqueueCommand(command); break; }; case CMD_INIT_POSE: { ///@todo: implement this command.m_type = CMD_INIT_POSE; cl->enqueueCommand(command); break; } case CMD_CREATE_BOX_COLLISION_SHAPE: { command.m_type =CMD_CREATE_BOX_COLLISION_SHAPE; cl->enqueueCommand(command); break; } case CMD_REQUEST_ACTUAL_STATE: { command.m_type =CMD_REQUEST_ACTUAL_STATE; cl->enqueueCommand(command); break; }; case CMD_STEP_FORWARD_SIMULATION: { command.m_type =CMD_STEP_FORWARD_SIMULATION; cl->enqueueCommand(command); break; } case CMD_SEND_DESIRED_STATE: { command.m_type =CMD_SEND_DESIRED_STATE; int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE; command.m_sendDesiredStateCommandArgument.m_controlMode = controlMode; //todo: expose a drop box in the GUI for this switch (controlMode) { case CONTROL_MODE_VELOCITY: { for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++) { command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0; command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000; } for (int i=0;i<cl->m_numMotors;i++) { btScalar targetVel = cl->m_motorTargetVelocities[i].m_velTarget; int uIndex = cl->m_motorTargetVelocities[i].m_uIndex; if (targetVel>1) { printf("testme"); } command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel; } break; } case CONTROL_MODE_TORQUE: { for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++) { command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 100; } break; } default: { b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE"); btAssert(0); } } cl->enqueueCommand(command); break; } case CMD_SEND_BULLET_DATA_STREAM: { command.m_type = buttonId; cl->enqueueCommand(command); break; } default: { b3Error("Unknown buttonId"); btAssert(0); } }; }