2015-07-15 19:57:45 +00:00
# include "RobotControlExample.h"
2015-09-17 06:09:10 +00:00
#if 0
2015-07-15 19:57:45 +00:00
# include "../CommonInterfaces/CommonParameterInterface.h"
# include "PhysicsServer.h"
# include "PhysicsClient.h"
# include "SharedMemoryCommon.h"
2015-07-15 22:23:40 +00:00
# include "../Utils/b3Clock.h"
2015-07-23 18:51:25 +00:00
# include "PhysicsClientC_API.h"
2015-08-20 05:51:16 +00:00
# include "../Utils/b3ResourcePath.h"
2015-08-24 22:26:29 +00:00
# include <string>
2015-07-15 22:23:40 +00:00
2015-07-15 19:57:45 +00:00
//const char* blaatnaam = "basename";
2015-09-17 06:09:10 +00:00
# define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE 1024
2015-07-15 19:57:45 +00:00
2015-07-21 06:35:29 +00:00
struct MyMotorInfo
{
2015-08-24 22:26:29 +00:00
std : : string m_jointName ;
2015-07-21 06:35:29 +00:00
btScalar m_velTarget ;
2015-08-20 05:51:16 +00:00
btScalar m_posTarget ;
2015-08-20 21:57:14 +00:00
btScalar m_kp ;
btScalar m_kd ;
2015-07-21 06:35:29 +00:00
btScalar m_maxForce ;
int m_uIndex ;
2015-08-20 05:51:16 +00:00
int m_posIndex ;
2015-08-24 22:26:29 +00:00
int m_jointIndex ;
btScalar m_measuredJointPosition ;
btScalar m_measuredJointVelocity ;
btVector3 m_measuredJointForce ;
btVector3 m_measuredJointTorque ;
2015-08-20 05:51:16 +00:00
2015-07-21 06:35:29 +00:00
} ;
# define MAX_NUM_MOTORS 128
2015-07-15 19:57:45 +00:00
class RobotControlExample : public SharedMemoryCommon
{
PhysicsServerSharedMemory m_physicsServer ;
PhysicsClientSharedMemory m_physicsClient ;
2015-07-15 22:23:40 +00:00
b3Clock m_realtimeClock ;
int m_sequenceNumberGenerator ;
2015-07-15 19:57:45 +00:00
bool m_wantsShutdown ;
2015-07-15 22:23:40 +00:00
btAlignedObjectArray < SharedMemoryCommand > m_userCommandRequests ;
2015-08-24 22:26:29 +00:00
2015-07-21 06:35:29 +00:00
2015-07-15 19:57:45 +00:00
void createButton ( const char * name , int id , bool isTrigger ) ;
public :
2015-07-21 06:35:29 +00:00
//@todo, add accessor methods
2015-08-20 05:51:16 +00:00
MyMotorInfo m_motorTargetState [ MAX_NUM_MOTORS ] ;
2015-07-21 06:35:29 +00:00
int m_numMotors ;
2015-08-20 05:51:16 +00:00
int m_option ;
bool m_verboseOutput ;
2015-07-21 06:35:29 +00:00
2015-08-20 05:51:16 +00:00
RobotControlExample ( GUIHelperInterface * helper , int option ) ;
2015-07-15 19:57:45 +00:00
virtual ~ RobotControlExample ( ) ;
virtual void initPhysics ( ) ;
virtual void stepSimulation ( float deltaTime ) ;
2015-08-20 05:51:16 +00:00
void prepareControlCommand ( SharedMemoryCommand & cmd ) ;
2015-07-15 22:23:40 +00:00
void enqueueCommand ( const SharedMemoryCommand & orgCommand )
2015-07-15 19:57:45 +00:00
{
2015-07-15 22:23:40 +00:00
m_userCommandRequests . push_back ( orgCommand ) ;
SharedMemoryCommand & cmd = m_userCommandRequests [ m_userCommandRequests . size ( ) - 1 ] ;
cmd . m_sequenceNumber = m_sequenceNumberGenerator + + ;
cmd . m_timeStamp = m_realtimeClock . getTimeMicroseconds ( ) ;
2015-08-20 05:51:16 +00:00
if ( m_verboseOutput )
{
b3Printf ( " User put command request %d on queue (queue length = %d) \n " , cmd . m_type , m_userCommandRequests . size ( ) ) ;
}
2015-07-15 19:57:45 +00:00
}
virtual void resetCamera ( )
{
float dist = 5 ;
float pitch = 50 ;
float yaw = 35 ;
float targetPos [ 3 ] = { 0 , 0 , 0 } ; //-3,2.8,-2.5};
m_guiHelper - > resetCamera ( dist , pitch , yaw , targetPos [ 0 ] , targetPos [ 1 ] , targetPos [ 2 ] ) ;
}
virtual bool wantsTermination ( ) ;
2015-08-06 18:59:31 +00:00
virtual bool isConnected ( ) ;
2015-08-07 20:17:33 +00:00
virtual void renderScene ( )
{
m_physicsServer . renderScene ( ) ;
}
2015-08-07 07:13:26 +00:00
virtual void exitPhysics ( ) { }
2015-08-07 20:17:33 +00:00
virtual void physicsDebugDraw ( int debugFlags )
{
m_physicsServer . physicsDebugDraw ( debugFlags ) ;
2015-08-20 05:51:16 +00:00
2015-08-07 20:17:33 +00:00
}
2015-08-07 07:13:26 +00:00
virtual bool mouseMoveCallback ( float x , float y ) { return false ; } ;
virtual bool mouseButtonCallback ( int button , int state , float x , float y ) { return false ; }
virtual bool keyboardCallback ( int key , int state ) { return false ; }
virtual void setSharedMemoryKey ( int key )
{
m_physicsServer . setSharedMemoryKey ( key ) ;
m_physicsClient . setSharedMemoryKey ( key ) ;
}
2015-07-15 19:57:45 +00:00
} ;
2015-08-06 18:59:31 +00:00
bool RobotControlExample : : isConnected ( )
{
return m_physicsClient . isConnected ( ) ;
}
2015-07-15 19:57:45 +00:00
void MyCallback2 ( int buttonId , bool buttonState , void * userPtr )
{
RobotControlExample * cl = ( RobotControlExample * ) userPtr ;
2015-07-15 22:23:40 +00:00
SharedMemoryCommand command ;
2015-07-15 19:57:45 +00:00
switch ( buttonId )
{
case CMD_LOAD_URDF :
2015-07-15 22:23:40 +00:00
{
command . m_type = CMD_LOAD_URDF ;
2015-08-07 20:17:33 +00:00
command . m_updateFlags = URDF_ARGS_FILE_NAME | URDF_ARGS_INITIAL_POSITION | URDF_ARGS_INITIAL_ORIENTATION ;
2015-09-03 21:18:22 +00:00
sprintf ( command . m_urdfArguments . m_urdfFileName , " r2d2.urdf " ) ; //kuka_lwr/kuka.urdf");//r2d2.urdf");
2015-07-15 22:23:40 +00:00
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 ;
}
2015-07-22 04:46:28 +00:00
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS :
{
2015-07-23 18:51:25 +00:00
//#ifdef USE_C_API
b3InitPhysicsParamCommand ( & command ) ;
2015-08-20 05:51:16 +00:00
b3PhysicsParamSetGravity ( & command , 1 , 1 , - 10 ) ;
2015-07-23 18:51:25 +00:00
// #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
2015-07-22 04:46:28 +00:00
cl - > enqueueCommand ( command ) ;
break ;
} ;
case CMD_INIT_POSE :
{
///@todo: implement this
command . m_type = CMD_INIT_POSE ;
cl - > enqueueCommand ( command ) ;
break ;
}
2015-07-15 19:57:45 +00:00
case CMD_CREATE_BOX_COLLISION_SHAPE :
2015-07-15 22:23:40 +00:00
{
command . m_type = CMD_CREATE_BOX_COLLISION_SHAPE ;
2015-08-06 18:59:31 +00:00
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 ;
2015-07-15 22:23:40 +00:00
cl - > enqueueCommand ( command ) ;
break ;
}
2015-07-15 19:57:45 +00:00
case CMD_REQUEST_ACTUAL_STATE :
2015-07-15 22:23:40 +00:00
{
command . m_type = CMD_REQUEST_ACTUAL_STATE ;
cl - > enqueueCommand ( command ) ;
break ;
} ;
2015-07-15 19:57:45 +00:00
case CMD_STEP_FORWARD_SIMULATION :
2015-07-15 22:23:40 +00:00
{
command . m_type = CMD_STEP_FORWARD_SIMULATION ;
cl - > enqueueCommand ( command ) ;
break ;
}
2015-07-15 19:57:45 +00:00
case CMD_SEND_DESIRED_STATE :
2015-07-15 22:23:40 +00:00
{
2015-07-21 06:35:29 +00:00
2015-08-20 05:51:16 +00:00
command . m_type = CMD_SEND_DESIRED_STATE ;
cl - > prepareControlCommand ( command ) ;
2015-07-15 22:23:40 +00:00
cl - > enqueueCommand ( command ) ;
break ;
}
2015-07-15 19:57:45 +00:00
case CMD_SEND_BULLET_DATA_STREAM :
{
2015-07-15 22:23:40 +00:00
command . m_type = buttonId ;
2015-08-20 05:51:16 +00:00
sprintf ( command . m_dataStreamArguments . m_bulletFileName , " slope.bullet " ) ;
command . m_dataStreamArguments . m_streamChunkLength = 0 ;
2015-07-15 22:23:40 +00:00
cl - > enqueueCommand ( command ) ;
2015-07-15 19:57:45 +00:00
break ;
}
default :
{
b3Error ( " Unknown buttonId " ) ;
btAssert ( 0 ) ;
}
} ;
}
2015-08-20 05:51:16 +00:00
void RobotControlExample : : prepareControlCommand ( SharedMemoryCommand & command )
{
for ( int i = 0 ; i < MAX_DEGREE_OF_FREEDOM ; i + + )
{
command . m_sendDesiredStateCommandArgument . m_desiredStateQdot [ i ] = 0 ;
command . m_sendDesiredStateCommandArgument . m_desiredStateForceTorque [ i ] = 0 ;
}
switch ( m_option )
{
case ROBOT_VELOCITY_CONTROL :
{
command . m_sendDesiredStateCommandArgument . m_controlMode = 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 < m_numMotors ; i + + )
{
btScalar targetVel = m_motorTargetState [ i ] . m_velTarget ;
int uIndex = m_motorTargetState [ i ] . m_uIndex ;
command . m_sendDesiredStateCommandArgument . m_desiredStateQdot [ uIndex ] = targetVel ;
}
break ;
}
case ROBOT_PD_CONTROL :
{
command . m_sendDesiredStateCommandArgument . m_controlMode = CONTROL_MODE_POSITION_VELOCITY_PD ;
for ( int i = 0 ; i < m_numMotors ; i + + )
{
int uIndex = m_motorTargetState [ i ] . m_uIndex ;
2015-08-20 21:57:14 +00:00
command . m_sendDesiredStateCommandArgument . m_Kp [ uIndex ] = m_motorTargetState [ i ] . m_kp ;
command . m_sendDesiredStateCommandArgument . m_Kd [ uIndex ] = m_motorTargetState [ i ] . m_kd ;
command . m_sendDesiredStateCommandArgument . m_desiredStateForceTorque [ uIndex ] = 10000 ; //max force
2015-08-20 05:51:16 +00:00
btScalar targetVel = m_motorTargetState [ i ] . m_velTarget ;
command . m_sendDesiredStateCommandArgument . m_desiredStateQdot [ uIndex ] = targetVel ;
int posIndex = m_motorTargetState [ i ] . m_posIndex ;
btScalar targetPos = m_motorTargetState [ i ] . m_posTarget ;
command . m_sendDesiredStateCommandArgument . m_desiredStateQ [ posIndex ] = targetPos ;
}
break ;
}
2015-08-24 22:26:29 +00:00
case ROBOT_PING_PONG_JOINT_FEEDBACK :
{
command . m_sendDesiredStateCommandArgument . m_controlMode = 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 < m_numMotors ; i + + )
{
btScalar targetVel = m_motorTargetState [ i ] . m_velTarget ;
int uIndex = m_motorTargetState [ i ] . m_uIndex ;
command . m_sendDesiredStateCommandArgument . m_desiredStateQdot [ uIndex ] = m_motorTargetState [ i ] . m_velTarget ;
}
break ;
}
2015-08-20 05:51:16 +00:00
default :
{
b3Warning ( " Unknown control mode in RobotControlExample::prepareControlCommand " ) ;
}
} ;
}
2015-07-15 19:57:45 +00:00
void RobotControlExample : : createButton ( const char * name , int buttonId , bool isTrigger )
{
ButtonParams button ( name , buttonId , isTrigger ) ;
button . m_callback = MyCallback2 ;
button . m_userPointer = this ;
m_guiHelper - > getParameterInterface ( ) - > registerButtonParameter ( button ) ;
}
2015-08-20 05:51:16 +00:00
RobotControlExample : : RobotControlExample ( GUIHelperInterface * helper , int option )
2015-07-15 19:57:45 +00:00
: SharedMemoryCommon ( helper ) ,
2015-07-15 22:23:40 +00:00
m_wantsShutdown ( false ) ,
2015-07-21 06:35:29 +00:00
m_sequenceNumberGenerator ( 0 ) ,
2015-08-20 05:51:16 +00:00
m_numMotors ( 0 ) ,
m_option ( option ) ,
m_verboseOutput ( false )
2015-07-15 19:57:45 +00:00
{
bool useServer = true ;
}
RobotControlExample : : ~ RobotControlExample ( )
{
bool deInitializeSharedMemory = true ;
2015-08-06 18:59:31 +00:00
m_physicsClient . disconnectSharedMemory ( ) ;
2015-07-15 19:57:45 +00:00
m_physicsServer . disconnectSharedMemory ( deInitializeSharedMemory ) ;
}
void RobotControlExample : : initPhysics ( )
{
///for this testing we use Z-axis up
int upAxis = 2 ;
m_guiHelper - > setUpAxis ( upAxis ) ;
2015-08-07 07:13:26 +00:00
/* createEmptyDynamicsWorld();
2015-07-15 19:57:45 +00:00
//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
m_guiHelper - > createPhysicsDebugDrawer ( m_dynamicsWorld ) ;
btVector3 grav ( 0 , 0 , 0 ) ;
grav [ upAxis ] = 0 ; //-9.8;
this - > m_dynamicsWorld - > setGravity ( grav ) ;
2015-08-07 07:13:26 +00:00
*/
m_physicsServer . connectSharedMemory ( m_guiHelper ) ;
2015-07-15 19:57:45 +00:00
if ( m_guiHelper & & m_guiHelper - > getParameterInterface ( ) )
{
bool isTrigger = false ;
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 ) ;
2015-07-22 04:46:28 +00:00
createButton ( " Set Physics Params " , CMD_SEND_PHYSICS_SIMULATION_PARAMETERS , isTrigger ) ;
createButton ( " Init Pose " , CMD_INIT_POSE , isTrigger ) ;
2015-07-15 19:57:45 +00:00
} else
{
2015-07-15 22:23:40 +00:00
/*
2015-07-15 19:57:45 +00:00
m_userCommandRequests . push_back ( CMD_LOAD_URDF ) ;
m_userCommandRequests . push_back ( CMD_REQUEST_ACTUAL_STATE ) ;
m_userCommandRequests . push_back ( CMD_SEND_DESIRED_STATE ) ;
m_userCommandRequests . push_back ( CMD_REQUEST_ACTUAL_STATE ) ;
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
m_userCommandRequests . push_back ( CMD_CREATE_BOX_COLLISION_SHAPE ) ;
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests . push_back ( CMD_STEP_FORWARD_SIMULATION ) ;
m_userCommandRequests . push_back ( CMD_REQUEST_ACTUAL_STATE ) ;
m_userCommandRequests . push_back ( CMD_SHUTDOWN ) ;
2015-07-15 22:23:40 +00:00
*/
2015-07-15 19:57:45 +00:00
}
if ( ! m_physicsClient . connect ( ) )
{
b3Warning ( " Cannot eonnect to physics client " ) ;
}
}
bool RobotControlExample : : wantsTermination ( )
{
return m_wantsShutdown ;
}
void RobotControlExample : : stepSimulation ( float deltaTime )
{
2015-08-20 05:51:16 +00:00
m_physicsServer . processClientCommands ( ) ;
2015-07-15 19:57:45 +00:00
if ( m_physicsClient . isConnected ( ) )
{
2015-07-21 06:35:29 +00:00
2015-07-23 18:51:25 +00:00
SharedMemoryStatus status ;
2015-07-21 06:35:29 +00:00
bool hasStatus = m_physicsClient . processServerStatus ( status ) ;
2015-08-31 19:36:08 +00:00
if ( ( m_option = = ROBOT_PING_PONG_JOINT_FEEDBACK ) & & hasStatus & & status . m_type = = CMD_ACTUAL_STATE_UPDATE_COMPLETED )
2015-08-24 22:26:29 +00:00
{
//update sensor feedback: joint force/torque data and measured joint positions
for ( int i = 0 ; i < m_numMotors ; i + + )
{
int jointIndex = m_motorTargetState [ i ] . m_jointIndex ;
int positionIndex = m_motorTargetState [ i ] . m_posIndex ;
int velocityIndex = m_motorTargetState [ i ] . m_uIndex ;
m_motorTargetState [ i ] . m_measuredJointPosition = status . m_sendActualStateArgs . m_actualStateQ [ positionIndex ] ;
m_motorTargetState [ i ] . m_measuredJointVelocity = status . m_sendActualStateArgs . m_actualStateQdot [ velocityIndex ] ;
m_motorTargetState [ i ] . m_measuredJointForce . setValue ( status . m_sendActualStateArgs . m_jointReactionForces [ 6 * jointIndex ] ,
status . m_sendActualStateArgs . m_jointReactionForces [ 6 * jointIndex + 1 ] ,
status . m_sendActualStateArgs . m_jointReactionForces [ 6 * jointIndex + 2 ] ) ;
m_motorTargetState [ i ] . m_measuredJointTorque . setValue ( status . m_sendActualStateArgs . m_jointReactionForces [ 6 * jointIndex + 3 ] ,
status . m_sendActualStateArgs . m_jointReactionForces [ 6 * jointIndex + 4 ] ,
status . m_sendActualStateArgs . m_jointReactionForces [ 6 * jointIndex + 5 ] ) ;
if ( m_motorTargetState [ i ] . m_measuredJointPosition > 0.1 )
{
m_motorTargetState [ i ] . m_velTarget = - 1.5 ;
} else
{
m_motorTargetState [ i ] . m_velTarget = 1.5 ;
}
b3Printf ( " Joint Force (Linear) [%s]=(%f,%f,%f) \n " , m_motorTargetState [ i ] . m_jointName . c_str ( ) , m_motorTargetState [ i ] . m_measuredJointForce . x ( ) , m_motorTargetState [ i ] . m_measuredJointForce . y ( ) , m_motorTargetState [ i ] . m_measuredJointForce . z ( ) ) ;
b3Printf ( " Joint Torque (Angular) [%s]=(%f,%f,%f) \n " , m_motorTargetState [ i ] . m_jointName . c_str ( ) , m_motorTargetState [ i ] . m_measuredJointTorque . x ( ) , m_motorTargetState [ i ] . m_measuredJointTorque . y ( ) , m_motorTargetState [ i ] . m_measuredJointTorque . z ( ) ) ;
}
}
2015-07-21 06:35:29 +00:00
if ( hasStatus & & status . m_type = = CMD_URDF_LOADING_COMPLETED )
{
2015-08-24 22:26:29 +00:00
SharedMemoryCommand sensorCommand ;
sensorCommand . m_type = CMD_CREATE_SENSOR ;
sensorCommand . m_createSensorArguments . m_numJointSensorChanges = 0 ;
for ( int jointIndex = 0 ; jointIndex < m_physicsClient . getNumJoints ( ) ; jointIndex + + )
2015-07-21 06:35:29 +00:00
{
2015-08-02 21:00:43 +00:00
b3JointInfo info ;
2015-08-24 22:26:29 +00:00
m_physicsClient . getJointInfo ( jointIndex , info ) ;
2015-08-20 05:51:16 +00:00
if ( m_verboseOutput )
{
b3Printf ( " Joint %s at q-index %d and u-index %d \n " , info . m_jointName , info . m_qIndex , info . m_uIndex ) ;
}
2015-07-21 06:35:29 +00:00
2015-08-02 21:00:43 +00:00
if ( info . m_flags & JOINT_HAS_MOTORIZED_POWER )
{
if ( m_numMotors < MAX_NUM_MOTORS )
{
2015-08-20 05:51:16 +00:00
switch ( m_option )
{
case ROBOT_VELOCITY_CONTROL :
{
char motorName [ 1024 ] ;
sprintf ( motorName , " %s q' " , info . m_jointName ) ;
MyMotorInfo * motorInfo = & m_motorTargetState [ m_numMotors ] ;
2015-08-24 22:26:29 +00:00
motorInfo - > m_jointName = info . m_jointName ;
2015-08-20 05:51:16 +00:00
motorInfo - > m_velTarget = 0.f ;
motorInfo - > m_posTarget = 0.f ;
2015-08-20 21:57:14 +00:00
2015-08-20 05:51:16 +00:00
motorInfo - > m_uIndex = info . m_uIndex ;
SliderParams slider ( motorName , & motorInfo - > m_velTarget ) ;
slider . m_minVal = - 4 ;
slider . m_maxVal = 4 ;
m_guiHelper - > getParameterInterface ( ) - > registerSliderFloatParameter ( slider ) ;
m_numMotors + + ;
break ;
}
case ROBOT_PD_CONTROL :
{
char motorName [ 1024 ] ;
2015-08-20 21:57:14 +00:00
2015-08-20 05:51:16 +00:00
MyMotorInfo * motorInfo = & m_motorTargetState [ m_numMotors ] ;
2015-08-24 22:26:29 +00:00
motorInfo - > m_jointName = info . m_jointName ;
2015-08-20 05:51:16 +00:00
motorInfo - > m_velTarget = 0.f ;
motorInfo - > m_posTarget = 0.f ;
motorInfo - > m_uIndex = info . m_uIndex ;
motorInfo - > m_posIndex = info . m_qIndex ;
2015-08-20 21:57:14 +00:00
motorInfo - > m_kp = 1 ;
motorInfo - > m_kd = 0 ;
{
sprintf ( motorName , " %s kp " , info . m_jointName ) ;
SliderParams slider ( motorName , & motorInfo - > m_kp ) ;
slider . m_minVal = 0 ;
slider . m_maxVal = 1 ;
m_guiHelper - > getParameterInterface ( ) - > registerSliderFloatParameter ( slider ) ;
}
{
sprintf ( motorName , " %s q " , info . m_jointName ) ;
2015-08-20 05:51:16 +00:00
SliderParams slider ( motorName , & motorInfo - > m_posTarget ) ;
slider . m_minVal = - SIMD_PI ;
slider . m_maxVal = SIMD_PI ;
m_guiHelper - > getParameterInterface ( ) - > registerSliderFloatParameter ( slider ) ;
2015-08-20 21:57:14 +00:00
}
{
sprintf ( motorName , " %s kd " , info . m_jointName ) ;
SliderParams slider ( motorName , & motorInfo - > m_kd ) ;
slider . m_minVal = 0 ;
slider . m_maxVal = 1 ;
m_guiHelper - > getParameterInterface ( ) - > registerSliderFloatParameter ( slider ) ;
}
{
sprintf ( motorName , " %s q' " , info . m_jointName ) ;
SliderParams slider ( motorName , & motorInfo - > m_velTarget ) ;
slider . m_minVal = - 10 ;
slider . m_maxVal = 10 ;
m_guiHelper - > getParameterInterface ( ) - > registerSliderFloatParameter ( slider ) ;
}
2015-08-20 05:51:16 +00:00
m_numMotors + + ;
break ;
}
2015-08-24 22:26:29 +00:00
case ROBOT_PING_PONG_JOINT_FEEDBACK :
{
if ( info . m_flags & JOINT_HAS_MOTORIZED_POWER )
{
if ( m_numMotors < MAX_NUM_MOTORS )
{
MyMotorInfo * motorInfo = & m_motorTargetState [ m_numMotors ] ;
motorInfo - > m_jointName = info . m_jointName ;
motorInfo - > m_velTarget = 0.f ;
motorInfo - > m_posTarget = 0.f ;
motorInfo - > m_uIndex = info . m_uIndex ;
motorInfo - > m_posIndex = info . m_qIndex ;
motorInfo - > m_jointIndex = jointIndex ;
2015-09-03 21:18:22 +00:00
sensorCommand . m_createSensorArguments . m_sensorType [ sensorCommand . m_createSensorArguments . m_numJointSensorChanges ] = SENSOR_FORCE_TORQUE ;
2015-08-24 22:26:29 +00:00
sensorCommand . m_createSensorArguments . m_jointIndex [ sensorCommand . m_createSensorArguments . m_numJointSensorChanges ] = jointIndex ;
sensorCommand . m_createSensorArguments . m_enableJointForceSensor [ sensorCommand . m_createSensorArguments . m_numJointSensorChanges ] = true ;
sensorCommand . m_createSensorArguments . m_numJointSensorChanges + + ;
m_numMotors + + ;
}
}
break ;
}
2015-08-20 05:51:16 +00:00
default :
{
b3Warning ( " Unknown control mode in RobotControlExample::stepSimulation " ) ;
}
} ;
2015-08-02 21:00:43 +00:00
}
}
2015-07-21 06:35:29 +00:00
}
2015-08-24 22:26:29 +00:00
if ( sensorCommand . m_createSensorArguments . m_numJointSensorChanges )
{
enqueueCommand ( sensorCommand ) ;
}
2015-07-21 06:35:29 +00:00
}
2015-08-24 22:26:29 +00:00
2015-07-15 19:57:45 +00:00
if ( m_physicsClient . canSubmitCommand ( ) )
{
if ( m_userCommandRequests . size ( ) )
{
2015-08-20 05:51:16 +00:00
if ( m_verboseOutput )
{
b3Printf ( " Outstanding user command requests: %d \n " , m_userCommandRequests . size ( ) ) ;
}
SharedMemoryCommand cmd = m_userCommandRequests [ 0 ] ;
2015-07-15 19:57:45 +00:00
//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 ( ) ;
2015-08-24 22:26:29 +00:00
if ( cmd . m_type = = CMD_CREATE_SENSOR )
{
b3Printf ( " CMD_CREATE_SENSOR! \n " ) ;
}
2015-08-20 05:51:16 +00:00
if ( cmd . m_type = = CMD_SEND_BULLET_DATA_STREAM )
{
char relativeFileName [ 1024 ] ;
bool fileFound = b3ResourcePath : : findResourcePath ( cmd . m_dataStreamArguments . m_bulletFileName , relativeFileName , 1024 ) ;
if ( fileFound )
{
FILE * fp = fopen ( relativeFileName , " rb " ) ;
if ( fp )
{
fseek ( fp , 0L , SEEK_END ) ;
int mFileLen = ftell ( fp ) ;
fseek ( fp , 0L , SEEK_SET ) ;
if ( mFileLen < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE )
{
char * data = ( char * ) malloc ( mFileLen ) ;
fread ( data , mFileLen , 1 , fp ) ;
fclose ( fp ) ;
cmd . m_dataStreamArguments . m_streamChunkLength = mFileLen ;
m_physicsClient . uploadBulletFileToSharedMemory ( data , mFileLen ) ;
if ( m_verboseOutput )
{
b3Printf ( " Loaded bullet data chunks into shared memory \n " ) ;
}
free ( data ) ;
} else
{
b3Warning ( " Bullet file size (%d) exceeds of streaming memory chunk size (%d) \n " , mFileLen , SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE ) ;
}
} else
{
b3Warning ( " Cannot open file %s \n " , relativeFileName ) ;
}
} else
{
b3Warning ( " Cannot find file %s \n " , cmd . m_dataStreamArguments . m_bulletFileName ) ;
}
}
2015-07-15 22:23:40 +00:00
m_physicsClient . submitClientCommand ( cmd ) ;
2015-08-20 05:51:16 +00:00
} else
{
if ( m_numMotors )
{
SharedMemoryCommand command ;
command . m_type = CMD_SEND_DESIRED_STATE ;
prepareControlCommand ( command ) ;
enqueueCommand ( command ) ;
command . m_type = CMD_STEP_FORWARD_SIMULATION ;
enqueueCommand ( command ) ;
2015-08-24 22:26:29 +00:00
command . m_type = CMD_REQUEST_ACTUAL_STATE ;
enqueueCommand ( command ) ;
2015-08-20 05:51:16 +00:00
}
2015-07-15 19:57:45 +00:00
}
}
}
}
2015-08-07 07:13:26 +00:00
extern int gSharedMemoryKey ;
2015-07-15 19:57:45 +00:00
class CommonExampleInterface * RobotControlExampleCreateFunc ( struct CommonExampleOptions & options )
{
2015-08-20 05:51:16 +00:00
RobotControlExample * example = new RobotControlExample ( options . m_guiHelper , options . m_option ) ;
2015-08-07 07:13:26 +00:00
if ( gSharedMemoryKey > = 0 )
{
example - > setSharedMemoryKey ( gSharedMemoryKey ) ;
}
return example ;
2015-07-15 19:57:45 +00:00
}
2015-09-17 06:09:10 +00:00
# endif
2015-07-15 19:57:45 +00:00