From 88aa9e899ef35703d49d320133a39196afd188d3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 24 Feb 2017 21:40:43 -0800 Subject: [PATCH] tweak Minitaur/RobotSimulator, fix target value from int to double --- examples/RobotSimulator/MinitaurSetup.cpp | 108 ++++++++++++------ examples/RobotSimulator/MinitaurSetup.h | 3 +- .../RobotSimulator/RobotSimulatorMain.cpp | 8 +- .../b3RobotSimulatorClientAPI.cpp | 2 +- .../b3RobotSimulatorClientAPI.h | 2 +- 5 files changed, 78 insertions(+), 45 deletions(-) diff --git a/examples/RobotSimulator/MinitaurSetup.cpp b/examples/RobotSimulator/MinitaurSetup.cpp index edd233789..0d1238c87 100644 --- a/examples/RobotSimulator/MinitaurSetup.cpp +++ b/examples/RobotSimulator/MinitaurSetup.cpp @@ -27,47 +27,79 @@ MinitaurSetup::~MinitaurSetup() 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 - def resetPose(self): - #right front leg - self.disableAllMotors() - p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57) - p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2) - 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) + b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD); + controlArgs.m_maxTorqueValue = maxTorque; + controlArgs.m_kd = kd; + controlArgs.m_kp = kp; + controlArgs.m_targetPosition = desiredAngle; + sim->setJointMotorControl(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId[motorName],controlArgs); +} - #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 - p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57) - p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2) - p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57) - p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2) - 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) - self.setMotorAngleByName('motor_back_rightL_joint',-1.57) +void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim) +{ + //release all motors + int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId); + for (int i=0;isetJointMotorControl(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) @@ -90,7 +122,7 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V } } - resetPose(); + resetPose(sim); return m_data->m_quadrupedUniqueId; } \ No newline at end of file diff --git a/examples/RobotSimulator/MinitaurSetup.h b/examples/RobotSimulator/MinitaurSetup.h index a8e221e02..02cd0014d 100644 --- a/examples/RobotSimulator/MinitaurSetup.h +++ b/examples/RobotSimulator/MinitaurSetup.h @@ -6,6 +6,7 @@ class MinitaurSetup { struct MinitaurSetupInternalData* m_data; + void resetPose(class b3RobotSimulatorClientAPI* sim); public: 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)); - 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 diff --git a/examples/RobotSimulator/RobotSimulatorMain.cpp b/examples/RobotSimulator/RobotSimulatorMain.cpp index dbea6bdf7..103bc0245 100644 --- a/examples/RobotSimulator/RobotSimulatorMain.cpp +++ b/examples/RobotSimulator/RobotSimulatorMain.cpp @@ -15,7 +15,7 @@ int main(int argc, char* argv[]) //Can also use eCONNECT_DIRECT,eCONNECT_SHARED_MEMORY,eCONNECT_UDP,eCONNECT_TCP, for example: //sim->connect(eCONNECT_UDP, "localhost", 1234); 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 sim->syncBodies(); @@ -27,7 +27,7 @@ int main(int argc, char* argv[]) sim->loadURDF("plane.urdf"); MinitaurSetup minitaur; - int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,1)); + int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3)); b3RobotSimulatorLoadUrdfFileArgs args; @@ -43,12 +43,12 @@ int main(int argc, char* argv[]) b3Clock clock; double startTime = clock.getTimeInSeconds(); double simWallClockSeconds = 20.; - +#if 0 while (clock.getTimeInSeconds()-startTime < simWallClockSeconds) { sim->stepSimulation(); } - +#endif sim->setRealTimeSimulation(true); startTime = clock.getTimeInSeconds(); diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index b897abf29..8159833c6 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -512,7 +512,7 @@ bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, return false; } -bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, int targetValue) +bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue) { b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index 09951ed32..4c11db00d 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -161,7 +161,7 @@ public: 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);