Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

tweak Minitaur/RobotSimulator, fix target value from int to double

Browse Source
This commit is contained in:
Erwin Coumans
2017-02-24 21:40:43 -08:00
parent a4f1e34899
commit 88aa9e899e
5 changed files with 78 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
examples/RobotSimulator/MinitaurSetup.cpp
View File

@@ -27,47 +27,79 @@ MinitaurSetup::~MinitaurSetup()
delete m_data; delete m_data;
} }
void MinitaurSetup::resetPose() void MinitaurSetup::setDesiredMotorAngle(class b3RobotSimulatorClientAPI* sim, const char* motorName, double desiredAngle, double maxTorque, double kp, double kd)
{ {
#if 0 b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD);
def resetPose(self): controlArgs.m_maxTorqueValue = maxTorque;
#right front leg controlArgs.m_kd = kd;
self.disableAllMotors() controlArgs.m_kp = kp;
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57) controlArgs.m_targetPosition = desiredAngle;
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2) sim->setJointMotorControl(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId[motorName],controlArgs);
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57) }
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
self.setMotorAngleByName('motor_front_rightR_joint', 1.57)
self.setMotorAngleByName('motor_front_rightL_joint',-1.57)
#left front leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
self.setMotorAngleByName('motor_front_leftR_joint', 1.57)
self.setMotorAngleByName('motor_front_leftL_joint',-1.57)
#right back leg void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57) {
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2) //release all motors
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57) int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2) for (int i=0;i<numJoints;i++)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2]) {
self.setMotorAngleByName('motor_back_rightR_joint', 1.57) b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
self.setMotorAngleByName('motor_back_rightL_joint',-1.57) controlArgs.m_maxTorqueValue = 0;
sim->setJointMotorControl(m_data->m_quadrupedUniqueId,i,controlArgs);
}
//right front leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],-2.2);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],-1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],2.2);
b3JointInfo jointInfo;
jointInfo.m_jointType = ePoint2PointType;
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.01; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = -0.015; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_front_rightR_joint",1.57);
setDesiredMotorAngle(sim,"motor_front_rightL_joint",-1.57);
//left front leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],-2.2);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftL_joint"],-1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],2.2);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = -0.01; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.015; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_front_leftR_joint", 1.57);
setDesiredMotorAngle(sim,"motor_front_leftL_joint", -1.57);
//right back leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightR_joint"],1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],-2.2);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightL_joint"],-1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],2.2);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.01; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = -0.015; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_back_rightR_joint", 1.57);
setDesiredMotorAngle(sim,"motor_back_rightL_joint", -1.57);
//left back leg
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftR_joint"],1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],-2.2);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftL_joint"],-1.57);
sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],2.2);
jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = -0.01; jointInfo.m_parentFrame[2] = 0.2;
jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.015; jointInfo.m_childFrame[2] = 0.2;
sim->createConstraint(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],
m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],&jointInfo);
setDesiredMotorAngle(sim,"motor_back_leftR_joint", 1.57);
setDesiredMotorAngle(sim,"motor_back_leftL_joint", -1.57);
#left back leg
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2)
p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57)
p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2)
p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
self.setMotorAngleByName('motor_back_leftR_joint', 1.57)
self.setMotorAngleByName('motor_back_leftL_joint',-1.57)
#endif
} }
int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn) int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn)
@@ -90,7 +122,7 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V
} }
} }
resetPose(); resetPose(sim);
return m_data->m_quadrupedUniqueId; return m_data->m_quadrupedUniqueId;
} }

3
examples/RobotSimulator/MinitaurSetup.h
View File

@@ -6,6 +6,7 @@
class MinitaurSetup class MinitaurSetup
{ {
struct MinitaurSetupInternalData* m_data; struct MinitaurSetupInternalData* m_data;
void resetPose(class b3RobotSimulatorClientAPI* sim);
public: public:
MinitaurSetup(); MinitaurSetup();
@@ -13,7 +14,7 @@ public:
int setupMinitaur(class b3RobotSimulatorClientAPI* sim, const class b3Vector3& startPos=b3MakeVector3(0,0,0), const class b3Quaternion& startOrn = b3Quaternion(0,0,0,1)); int setupMinitaur(class b3RobotSimulatorClientAPI* sim, const class b3Vector3& startPos=b3MakeVector3(0,0,0), const class b3Quaternion& startOrn = b3Quaternion(0,0,0,1));
void resetPose(); void setDesiredMotorAngle(class b3RobotSimulatorClientAPI* sim, const char* motorName, double desiredAngle, double maxTorque=3,double kp=0.1, double kd=0.9);
}; };
#endif //MINITAUR_SIMULATION_SETUP_H #endif //MINITAUR_SIMULATION_SETUP_H

8
examples/RobotSimulator/RobotSimulatorMain.cpp
View File

@@ -15,7 +15,7 @@ int main(int argc, char* argv[])
//Can also use eCONNECT_DIRECT,eCONNECT_SHARED_MEMORY,eCONNECT_UDP,eCONNECT_TCP, for example: //Can also use eCONNECT_DIRECT,eCONNECT_SHARED_MEMORY,eCONNECT_UDP,eCONNECT_TCP, for example:
//sim->connect(eCONNECT_UDP, "localhost", 1234); //sim->connect(eCONNECT_UDP, "localhost", 1234);
sim->configureDebugVisualizer( COV_ENABLE_GUI, 0); sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME // sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME
//syncBodies is only needed when connecting to an existing physics server that has already some bodies //syncBodies is only needed when connecting to an existing physics server that has already some bodies
sim->syncBodies(); sim->syncBodies();
@@ -27,7 +27,7 @@ int main(int argc, char* argv[])
sim->loadURDF("plane.urdf"); sim->loadURDF("plane.urdf");
MinitaurSetup minitaur; MinitaurSetup minitaur;
int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,1)); int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3));
b3RobotSimulatorLoadUrdfFileArgs args; b3RobotSimulatorLoadUrdfFileArgs args;
@@ -43,12 +43,12 @@ int main(int argc, char* argv[])
b3Clock clock; b3Clock clock;
double startTime = clock.getTimeInSeconds(); double startTime = clock.getTimeInSeconds();
double simWallClockSeconds = 20.; double simWallClockSeconds = 20.;
#if 0
while (clock.getTimeInSeconds()-startTime < simWallClockSeconds) while (clock.getTimeInSeconds()-startTime < simWallClockSeconds)
{ {
sim->stepSimulation(); sim->stepSimulation();
} }
#endif
sim->setRealTimeSimulation(true); sim->setRealTimeSimulation(true);
startTime = clock.getTimeInSeconds(); startTime = clock.getTimeInSeconds();

2
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@@ -512,7 +512,7 @@ bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex,
return false; return false;
} }
bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, int targetValue) bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
{ {
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;

2
examples/RobotSimulator/b3RobotSimulatorClientAPI.h
View File

@@ -161,7 +161,7 @@ public:
bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state); bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state);
bool resetJointState(int bodyUniqueId, int jointIndex, int targetValue); bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue);
void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args); void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args);