From 6e2850e08a59aab8846e54c82a2b3e5edc9e5bfa Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Tue, 14 Mar 2017 17:03:11 -0700 Subject: [PATCH 1/4] fix names (case) for Linux for MPL/hand --- data/MPL/MPL.xml | 48 +++++++++++++++++++-------------------- examples/pybullet/hand.py | 4 ++-- 2 files changed, 26 insertions(+), 26 deletions(-) diff --git a/data/MPL/MPL.xml b/data/MPL/MPL.xml index 2b8408323..7ed5170ff 100644 --- a/data/MPL/MPL.xml +++ b/data/MPL/MPL.xml @@ -49,30 +49,30 @@ - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/pybullet/hand.py b/examples/pybullet/hand.py index d0428c555..cb4b45f87 100644 --- a/examples/pybullet/hand.py +++ b/examples/pybullet/hand.py @@ -16,7 +16,7 @@ if (c<0): p.connect(p.GUI) #load the MuJoCo MJCF hand -objects = p.loadMJCF("MPL/mpl.xml") +objects = p.loadMJCF("MPL/MPL.xml") hand=objects[0] #clamp in range 400-600 @@ -75,4 +75,4 @@ if (ser.isOpen()): #print(middle) #print(pink) #print(index) - #print(thumb) \ No newline at end of file + #print(thumb) From 4db6fa9e29aa463dd8830f2fafb57d89f689d517 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 15 Mar 2017 15:38:50 -0700 Subject: [PATCH 2/4] update minitaur.py to use minitaur.urdf (instead of quadruped.urdf), also sort the legs in the same order as real hardware added test urdf files for minitaur with all fixed joints, or fixed knees. added some stiffness/damping to minitaur legs (testing) tiny_obj_loader, don't crash on invalid texture coordinates btMultiBodyConstraintSolver: sweep back and forward to reduce asymmetry --- data/quadruped/minitaur.urdf | 34 +- data/quadruped/minitaur_fixed_all.urdf | 913 +++++++++++++++++ data/quadruped/minitaur_fixed_knees.urdf | 929 ++++++++++++++++++ .../b3RobotSimulatorClientAPI.h | 2 +- .../Wavefront/tiny_obj_loader.cpp | 14 +- examples/pybullet/minitaur.py | 89 +- examples/pybullet/minitaur_evaluate.py | 2 +- examples/pybullet/minitaur_test.py | 4 + examples/pybullet/mylittleminitaur.py | 21 + examples/pybullet/quadruped.py | 141 +-- .../btMultiBodyConstraintSolver.cpp | 12 +- 11 files changed, 2043 insertions(+), 118 deletions(-) create mode 100644 data/quadruped/minitaur_fixed_all.urdf create mode 100644 data/quadruped/minitaur_fixed_knees.urdf create mode 100644 examples/pybullet/mylittleminitaur.py diff --git a/data/quadruped/minitaur.urdf b/data/quadruped/minitaur.urdf index c483bd4b4..663ff2a84 100644 --- a/data/quadruped/minitaur.urdf +++ b/data/quadruped/minitaur.urdf @@ -427,7 +427,9 @@ - + + + @@ -493,7 +495,9 @@ - + + + @@ -557,7 +561,9 @@ - + + + @@ -622,7 +628,9 @@ - + + + @@ -684,7 +692,9 @@ - + + + @@ -750,9 +760,11 @@ - + + + - + @@ -814,7 +826,9 @@ - + + + @@ -877,7 +891,9 @@ - + + + diff --git a/data/quadruped/minitaur_fixed_all.urdf b/data/quadruped/minitaur_fixed_all.urdf new file mode 100644 index 000000000..68815043a --- /dev/null +++ b/data/quadruped/minitaur_fixed_all.urdf @@ -0,0 +1,913 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/data/quadruped/minitaur_fixed_knees.urdf b/data/quadruped/minitaur_fixed_knees.urdf new file mode 100644 index 000000000..2c5903e1f --- /dev/null +++ b/data/quadruped/minitaur_fixed_knees.urdf @@ -0,0 +1,929 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index f1ca5fe98..e6f658cc5 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -146,7 +146,7 @@ public: b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat); int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs()); - bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs()); + bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results=b3RobotSimulatorLoadFileResults(), const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs()); bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); diff --git a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp index f69fa3dda..f56de333a 100644 --- a/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp +++ b/examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp @@ -218,8 +218,18 @@ updateVertex( } if (i.vt_idx >= 0) { - texcoords.push_back(in_texcoords[2*i.vt_idx+0]); - texcoords.push_back(in_texcoords[2*i.vt_idx+1]); + int numTexCoords = in_texcoords.size(); + int index0 = 2*i.vt_idx+0; + int index1 = 2*i.vt_idx+1; + + if (index0>=0 && (index0)=0 && (index1)m_multiBodyFrictionContactConstraints.size();j++) + for (int j1=0;j1m_multiBodyFrictionContactConstraints.size();j1++) { if (iteration < infoGlobal.m_numIterations) { - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[j]; + int index = iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; //adjust friction limits here if (totalImpulse>btScalar(0)) From b7b46b12d33a7d47ceaf8723a2d77cd03cc12dc3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 15 Mar 2017 17:09:17 -0700 Subject: [PATCH 3/4] update RobotSimulator/MinitaurSetup to use data/quadruped/minitaur.urdf add b3RobotSimulatorClientAPI::getBaseVelocity and resetBaseVelocity add b3Quaternion::getEulerZYX --- examples/RobotSimulator/MinitaurSetup.cpp | 98 ++-- .../RobotSimulator/RobotSimulatorMain.cpp | 36 +- .../b3RobotSimulatorClientAPI.cpp | 522 +++++++++--------- .../b3RobotSimulatorClientAPI.h | 118 ++-- src/Bullet3Common/b3Quaternion.h | 34 +- 5 files changed, 427 insertions(+), 381 deletions(-) diff --git a/examples/RobotSimulator/MinitaurSetup.cpp b/examples/RobotSimulator/MinitaurSetup.cpp index 0d1238c87..e867d0583 100644 --- a/examples/RobotSimulator/MinitaurSetup.cpp +++ b/examples/RobotSimulator/MinitaurSetup.cpp @@ -49,57 +49,69 @@ void MinitaurSetup::resetPose(class b3RobotSimulatorClientAPI* sim) 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); + b3Scalar startAngle = B3_HALF_PI; + b3Scalar upperLegLength = 11.5; + b3Scalar lowerLegLength = 20; + b3Scalar kneeAngle = B3_PI+b3Acos(upperLegLength/lowerLegLength); + + b3Scalar motorDirs[8] = {-1,-1,-1,-1,1,1,1,1}; 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; + //left front leg + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftL_joint"],motorDirs[0] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftL_link"],motorDirs[0]*kneeAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_leftR_joint"],motorDirs[1] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_leftR_link"],motorDirs[1]*kneeAngle); + + jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2; + jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; 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); - + setDesiredMotorAngle(sim,"motor_front_leftL_joint", motorDirs[0] * startAngle); + setDesiredMotorAngle(sim,"motor_front_leftR_joint", motorDirs[1] * startAngle); + //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->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftL_joint"],motorDirs[2] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftL_link"],motorDirs[2] * kneeAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_leftR_joint"],motorDirs[3] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_leftR_link"],motorDirs[3] * kneeAngle); + jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2; + jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; 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); + setDesiredMotorAngle(sim,"motor_back_leftL_joint", motorDirs[2] * startAngle); + setDesiredMotorAngle(sim,"motor_back_leftR_joint", motorDirs[3] * startAngle); + //right front leg + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightL_joint"],motorDirs[4] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightL_link"],motorDirs[4] * kneeAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_front_rightR_joint"],motorDirs[5]*startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_front_rightR_link"],motorDirs[5] * kneeAngle); + + + + jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2; + jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; 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_rightL_joint",motorDirs[4] * startAngle); + setDesiredMotorAngle(sim,"motor_front_rightR_joint",motorDirs[5] * startAngle); + + + //right back leg + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightL_joint"],motorDirs[6] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightL_link"],motorDirs[6] * kneeAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["motor_back_rightR_joint"],motorDirs[7] * startAngle); + sim->resetJointState(m_data->m_quadrupedUniqueId,*m_data->m_jointNameToId["knee_back_rightR_link"],motorDirs[7] * kneeAngle); + + jointInfo.m_parentFrame[0] = 0; jointInfo.m_parentFrame[1] = 0.005; jointInfo.m_parentFrame[2] = 0.2; + jointInfo.m_childFrame[0] = 0; jointInfo.m_childFrame[1] = 0.01; 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_rightL_joint", motorDirs[6] * startAngle); + setDesiredMotorAngle(sim,"motor_back_rightR_joint", motorDirs[7] * startAngle); + } int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn) @@ -109,7 +121,7 @@ int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3V args.m_startPosition = startPos; args.m_startOrientation = startOrn; - m_data->m_quadrupedUniqueId = sim->loadURDF("quadruped/quadruped.urdf",args); + m_data->m_quadrupedUniqueId = sim->loadURDF("quadruped/minitaur.urdf",args); int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId); for (int i=0;isyncBodies(); - - sim->setTimeStep(1./240.); + b3Scalar fixedTimeStep = 1./240.; - sim->setGravity(b3MakeVector3(0,0,-10)); + sim->setTimeStep(fixedTimeStep); - int blockId = sim->loadURDF("cube.urdf"); - b3BodyInfo bodyInfo; - sim->getBodyInfo(blockId,&bodyInfo); + b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3)); + b3Vector3 rpy; + rpy = sim->getEulerFromQuaternion(q); + + sim->setGravity(b3MakeVector3(0,0,-9.8)); + + //int blockId = sim->loadURDF("cube.urdf"); + //b3BodyInfo bodyInfo; + //sim->getBodyInfo(blockId,&bodyInfo); sim->loadURDF("plane.urdf"); @@ -34,15 +39,15 @@ int main(int argc, char* argv[]) int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,.3)); - b3RobotSimulatorLoadUrdfFileArgs args; - args.m_startPosition.setValue(2,0,1); - int r2d2 = sim->loadURDF("r2d2.urdf",args); + //b3RobotSimulatorLoadUrdfFileArgs args; + //args.m_startPosition.setValue(2,0,1); + //int r2d2 = sim->loadURDF("r2d2.urdf",args); - b3RobotSimulatorLoadFileResults sdfResults; - if (!sim->loadSDF("two_cubes.sdf",sdfResults)) - { - b3Warning("Can't load SDF!\n"); - } + //b3RobotSimulatorLoadFileResults sdfResults; + //if (!sim->loadSDF("two_cubes.sdf",sdfResults)) + //{ +// b3Warning("Can't load SDF!\n"); + //} b3Clock clock; double startTime = clock.getTimeInSeconds(); @@ -69,7 +74,8 @@ int main(int argc, char* argv[]) printf("keyEvent[%d].m_keyCode = %d, state = %d\n", i,keyEvents.m_keyboardEvents[i].m_keyCode,keyEvents.m_keyboardEvents[i].m_keyState); } } - b3Clock::usleep(1000*1000); + sim->stepSimulation(); + b3Clock::usleep(1000.*1000.*fixedTimeStep); } printf("sim->disconnect\n"); diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 224014b76..4c95006d2 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -1,17 +1,16 @@ #include "b3RobotSimulatorClientAPI.h" - //#include "SharedMemoryCommands.h" #include "SharedMemory/PhysicsClientC_API.h" #ifdef BT_ENABLE_ENET #include "SharedMemory/PhysicsClientUDP_C_API.h" -#endif//PHYSICS_UDP +#endif //PHYSICS_UDP #ifdef BT_ENABLE_CLSOCKET #include "SharedMemory/PhysicsClientTCP_C_API.h" -#endif//PHYSICS_TCP +#endif //PHYSICS_TCP #include "SharedMemory/PhysicsDirectC_API.h" @@ -25,7 +24,7 @@ struct b3RobotSimulatorClientAPI_InternalData b3PhysicsClientHandle m_physicsClientHandle; b3RobotSimulatorClientAPI_InternalData() - :m_physicsClientHandle(0) + : m_physicsClientHandle(0) { } }; @@ -35,17 +34,16 @@ b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI() m_data = new b3RobotSimulatorClientAPI_InternalData(); } - b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI() +b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI() { - delete m_data; + delete m_data; } - bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey) { if (m_data->m_physicsClientHandle) { - b3Warning ("Already connected, disconnect first."); + b3Warning("Already connected, disconnect first."); return false; } b3PhysicsClientHandle sm = 0; @@ -54,89 +52,88 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i int tcpPort = 6667; int key = SHARED_MEMORY_KEY; bool connected = false; - - switch (mode) + switch (mode) { - case eCONNECT_GUI: + case eCONNECT_GUI: { - int argc = 0; - char* argv[1] = {0}; - #ifdef __APPLE__ - sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); - #else - sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); - #endif - break; + int argc = 0; + char* argv[1] = {0}; +#ifdef __APPLE__ + sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv); +#else + sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv); +#endif + break; } - case eCONNECT_DIRECT: { - sm = b3ConnectPhysicsDirect(); - break; + case eCONNECT_DIRECT: + { + sm = b3ConnectPhysicsDirect(); + break; } - case eCONNECT_SHARED_MEMORY: { - if (portOrKey>=0) + case eCONNECT_SHARED_MEMORY: + { + if (portOrKey >= 0) { key = portOrKey; } - sm = b3ConnectSharedMemory(key); - break; + sm = b3ConnectSharedMemory(key); + break; } - case eCONNECT_UDP: - { - if (portOrKey>=0) + case eCONNECT_UDP: + { + if (portOrKey >= 0) { udpPort = portOrKey; } - #ifdef BT_ENABLE_ENET +#ifdef BT_ENABLE_ENET sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort); - #else - b3Warning("UDP is not enabled in this build"); - #endif //BT_ENABLE_ENET +#else + b3Warning("UDP is not enabled in this build"); +#endif //BT_ENABLE_ENET break; - } - case eCONNECT_TCP: + } + case eCONNECT_TCP: { - if (portOrKey>=0) + if (portOrKey >= 0) { tcpPort = portOrKey; } - #ifdef BT_ENABLE_CLSOCKET - +#ifdef BT_ENABLE_CLSOCKET + sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort); - #else +#else b3Warning("TCP is not enabled in this pybullet build"); - #endif //BT_ENABLE_CLSOCKET +#endif //BT_ENABLE_CLSOCKET break; } - - - default: { - b3Warning("connectPhysicsServer unexpected argument"); + default: + { + b3Warning("connectPhysicsServer unexpected argument"); } }; - if (sm) - { - m_data->m_physicsClientHandle = sm; - if (!b3CanSubmitCommand(m_data->m_physicsClientHandle)) - { - disconnect(); - return false; - } - return true; - } - return false; + if (sm) + { + m_data->m_physicsClientHandle = sm; + if (!b3CanSubmitCommand(m_data->m_physicsClientHandle)) + { + disconnect(); + return false; + } + return true; + } + return false; } bool b3RobotSimulatorClientAPI::isConnected() const { - return (m_data->m_physicsClientHandle!=0); + return (m_data->m_physicsClientHandle != 0); } - void b3RobotSimulatorClientAPI::disconnect() { if (!isConnected()) @@ -157,15 +154,13 @@ void b3RobotSimulatorClientAPI::syncBodies() return; } - - b3SharedMemoryCommandHandle command; + b3SharedMemoryCommandHandle command; b3SharedMemoryStatusHandle statusHandle; int statusType; command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusType = b3GetStatusType(statusHandle); - } void b3RobotSimulatorClientAPI::resetSimulation() @@ -175,9 +170,9 @@ void b3RobotSimulatorClientAPI::resetSimulation() b3Warning("Not connected"); return; } - b3SharedMemoryStatusHandle statusHandle; - statusHandle = b3SubmitClientCommandAndWaitStatus( - m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle)); + b3SharedMemoryStatusHandle statusHandle; + statusHandle = b3SubmitClientCommandAndWaitStatus( + m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle)); } bool b3RobotSimulatorClientAPI::canSubmitCommand() const @@ -186,7 +181,7 @@ bool b3RobotSimulatorClientAPI::canSubmitCommand() const { return false; } - return (b3CanSubmitCommand(m_data->m_physicsClientHandle)); + return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0); } void b3RobotSimulatorClientAPI::stepSimulation() @@ -200,11 +195,11 @@ void b3RobotSimulatorClientAPI::stepSimulation() b3SharedMemoryStatusHandle statusHandle; int statusType; if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle)); - statusType = b3GetStatusType(statusHandle); - b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED); - } + { + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle)); + statusType = b3GetStatusType(statusHandle); + b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED); + } } void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration) @@ -215,57 +210,26 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration) return; } - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetGravity(command, gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]); + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw) { - double phi, the, psi; - phi = rollPitchYaw[0] / 2.0; - the = rollPitchYaw[1] / 2.0; - psi = rollPitchYaw[2] / 2.0; - double quat[4] = { - sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi), - cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi), - cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi), - cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)}; - - // normalize the quaternion - double len = sqrt(quat[0] * quat[0] + quat[1] * quat[1] + - quat[2] * quat[2] + quat[3] * quat[3]); - quat[0] /= len; - quat[1] /= len; - quat[2] /= len; - quat[3] /= len; - b3Quaternion q(quat[0],quat[1],quat[2],quat[3]); + b3Quaternion q; + q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]); return q; } b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat) { - double squ; - double sqx; - double sqy; - double sqz; - double sarg; - - b3Vector3 rpy; - sqx = quat[0] * quat[0]; - sqy = quat[1] * quat[1]; - sqz = quat[2] * quat[2]; - squ = quat[3] * quat[3]; - rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]), - squ - sqx - sqy + sqz); - sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]); - rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538 - : (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg)); - rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]), - squ + sqx - sqy - sqz); - return rpy; + b3Scalar roll,pitch,yaw; + quat.getEulerZYX(yaw,pitch,roll); + b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw); + return rpy2; } int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args) @@ -278,31 +242,26 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc return robotUniqueId; } - - b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); - + //setting the initial position, orientation and other arguments are optional - + b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0], - args.m_startPosition[1], - args.m_startPosition[2]); - b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0] - ,args.m_startOrientation[1] - ,args.m_startOrientation[2] - ,args.m_startOrientation[3]); + args.m_startPosition[1], + args.m_startPosition[2]); + b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]); if (args.m_forceOverrideFixedBase) { - b3LoadUrdfCommandSetUseFixedBase(command,true); + b3LoadUrdfCommandSetUseFixedBase(command, true); } b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusType = b3GetStatusType(statusHandle); - b3Assert(statusType==CMD_URDF_LOADING_COMPLETED); - if (statusType==CMD_URDF_LOADING_COMPLETED) + b3Assert(statusType == CMD_URDF_LOADING_COMPLETED); + if (statusType == CMD_URDF_LOADING_COMPLETED) { robotUniqueId = b3GetStatusBodyIndex(statusHandle); } @@ -329,12 +288,12 @@ bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSim b3Warning("Couldn't load .mjcf file."); return false; } - int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); + int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); if (numBodies) { results.m_uniqueObjectIds.resize(numBodies); int numBodies; - numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); + numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size()); } return true; @@ -359,12 +318,12 @@ bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotS { return false; } - int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); + int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); if (numBodies) { results.m_uniqueObjectIds.resize(numBodies); int numBodies; - numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); + numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size()); } return true; @@ -383,19 +342,18 @@ bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimu b3SharedMemoryStatusHandle statusHandle; int statusType; b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str()); - b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); + b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); statusType = b3GetStatusType(statusHandle); b3Assert(statusType == CMD_SDF_LOADING_COMPLETED); if (statusType == CMD_SDF_LOADING_COMPLETED) { - int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0); + int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0); if (numBodies) { results.m_uniqueObjectIds.resize(numBodies); int numBodies; - numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size()); - + numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size()); } statusOk = true; } @@ -411,7 +369,7 @@ bool b3RobotSimulatorClientAPI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo* return false; } - int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo); + int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo); return (result != 0); } @@ -431,7 +389,8 @@ bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId, const int status_type = b3GetStatusType(status_handle); const double* actualStateQ; - if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { return false; } @@ -466,14 +425,72 @@ bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]); b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1], - baseOrientation[2], baseOrientation[3]); + baseOrientation[2], baseOrientation[3]); b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - + return true; } +bool b3RobotSimulatorClientAPI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + const int status_type = b3GetStatusType(statusHandle); + const double* actualStateQdot; + + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + return false; + } + + b3GetStatusActualState(statusHandle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, 0, + &actualStateQdot, 0 /* joint_reaction_forces */); + + baseLinearVelocity[0] = actualStateQdot[0]; + baseLinearVelocity[1] = actualStateQdot[1]; + baseLinearVelocity[2] = actualStateQdot[2]; + + baseAngularVelocity[0] = actualStateQdot[3]; + baseAngularVelocity[1] = actualStateQdot[4]; + baseAngularVelocity[2] = actualStateQdot[5]; + return true; +} + +bool b3RobotSimulatorClientAPI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const +{ + if (!isConnected()) + { + b3Warning("Not connected"); + return false; + } + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + + b3Vector3DoubleData linVelDouble; + linearVelocity.serializeDouble(linVelDouble); + b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats); + + b3Vector3DoubleData angVelDouble; + angularVelocity.serializeDouble(angVelDouble); + b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats); + + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + return true; +} void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation) { @@ -500,10 +517,9 @@ int b3RobotSimulatorClientAPI::getNumJoints(int bodyUniqueId) const b3Warning("Not connected"); return false; } - return b3GetNumJoints(m_data->m_physicsClientHandle,bodyUniqueId); + return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); } - bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo) { if (!isConnected()) @@ -511,7 +527,7 @@ bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b b3Warning("Not connected"); return false; } - return (b3GetJointInfo(m_data->m_physicsClientHandle,bodyUniqueId, jointIndex,jointInfo)!=0); + return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0); } void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo) @@ -521,55 +537,55 @@ void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parent b3Warning("Not connected"); return; } - b3SharedMemoryStatusHandle statusHandle; - b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); - if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) - { - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo)); - } + b3SharedMemoryStatusHandle statusHandle; + b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle)); + if (b3CanSubmitCommand(m_data->m_physicsClientHandle)) + { + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo)); + } } -bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state) +bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state) { if (!isConnected()) { b3Warning("Not connected"); return false; } - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - int statusType = b3GetStatusType(statusHandle); - if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + int statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) { - if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state)) - { - return true; - } + if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state)) + { + return true; + } } - return false; + return false; } bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue) { - b3SharedMemoryCommandHandle commandHandle; - b3SharedMemoryStatusHandle statusHandle; - int numJoints; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int numJoints; - numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); - if ((jointIndex >= numJoints) || (jointIndex < 0)) { - return false; - } + numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + return false; + } - commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); - b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex, - targetValue); + b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex, + targetValue); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); return false; } - void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args) { if (!isConnected()) @@ -582,39 +598,39 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint { case CONTROL_MODE_VELOCITY: { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY); + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY); b3JointInfo jointInfo; b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); int uIndex = jointInfo.m_uIndex; - b3JointControlSetKd(command,uIndex,args.m_kd); - b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); - b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); + b3JointControlSetKd(command, uIndex, args.m_kd); + b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity); + b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); break; } case CONTROL_MODE_POSITION_VELOCITY_PD: { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD); + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD); b3JointInfo jointInfo; b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); int uIndex = jointInfo.m_uIndex; int qIndex = jointInfo.m_qIndex; - b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition); - b3JointControlSetKp(command,uIndex,args.m_kp); - b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity); - b3JointControlSetKd(command,uIndex,args.m_kd); - b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue); + b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition); + b3JointControlSetKp(command, uIndex, args.m_kp); + b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity); + b3JointControlSetKd(command, uIndex, args.m_kd); + b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); break; } case CONTROL_MODE_TORQUE: { - b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE); + b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE); b3JointInfo jointInfo; b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo); int uIndex = jointInfo.m_uIndex; - b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue); + b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); break; } @@ -625,7 +641,6 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint } } - void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations) { if (!isConnected()) @@ -633,12 +648,11 @@ void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations) b3Warning("Not connected"); return; } - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetNumSolverIterations(command, numIterations); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); - + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetNumSolverIterations(command, numIterations); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds) @@ -654,10 +668,8 @@ void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds) int ret; ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - } - void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps) { if (!isConnected()) @@ -665,14 +677,13 @@ void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps) b3Warning("Not connected"); return; } - b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); - b3SharedMemoryStatusHandle statusHandle; - b3PhysicsParamSetNumSubSteps(command, numSubSteps); - statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetNumSubSteps(command, numSubSteps); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED); } - bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results) { if (!isConnected()) @@ -680,55 +691,52 @@ bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotS b3Warning("Not connected"); return false; } - b3Assert(args.m_endEffectorLinkIndex>=0); - b3Assert(args.m_bodyUniqueId>=0); - + b3Assert(args.m_endEffectorLinkIndex >= 0); + b3Assert(args.m_bodyUniqueId >= 0); - b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle,args.m_bodyUniqueId); + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle, args.m_bodyUniqueId); if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)) - { - b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); - } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) { - b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation); - } else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) - { - b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); - } else - { - b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition); + b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); + } + else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) + { + b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation); + } + else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY) + { + b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]); + } + else + { + b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition); } - - if (args.m_flags & B3_HAS_JOINT_DAMPING) - { - b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]); - } - b3SharedMemoryStatusHandle statusHandle; + if (args.m_flags & B3_HAS_JOINT_DAMPING) + { + b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]); + } + + b3SharedMemoryStatusHandle statusHandle; statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - + int numPos = 0; - int numPos=0; - bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - 0)!=0; - if (result && numPos) - { - results.m_calculatedJointPositions.resize(numPos); - result = b3GetStatusInverseKinematicsJointPositions(statusHandle, - &results.m_bodyUniqueId, - &numPos, - &results.m_calculatedJointPositions[0])!=0; - - } + &results.m_bodyUniqueId, + &numPos, + 0) != 0; + if (result && numPos) + { + results.m_calculatedJointPositions.resize(numPos); + result = b3GetStatusInverseKinematicsJointPositions(statusHandle, + &results.m_bodyUniqueId, + &numPos, + &results.m_calculatedJointPositions[0]) != 0; + } return result; } - - bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian) { if (!isConnected()) @@ -736,12 +744,12 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, b3Warning("Not connected"); return false; } - b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED) - { - b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian); + b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED) + { + b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian); return true; } return false; @@ -754,14 +762,14 @@ bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3 b3Warning("Not connected"); return false; } - b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId); - b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); - - if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) - { - b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState); + b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId); + b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command); + + if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED) + { + b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState); return true; - } + } return false; } @@ -787,9 +795,9 @@ void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData) return; } - b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle); + b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle); b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData); + b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData); } void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData) @@ -802,32 +810,35 @@ void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboard return; } - b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle); + b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle); b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); - b3GetKeyboardEventsData(m_data->m_physicsClientHandle,keyboardEventsData); - + b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData); } - -int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds) +int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds, int maxLogDof) { int loggingUniqueId = -1; b3SharedMemoryCommandHandle commandHandle; commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle); - - b3StateLoggingStart(commandHandle,loggingType,fileName.c_str()); - for ( int i=0;i 0) + { + b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof); } b3SharedMemoryStatusHandle statusHandle; int statusType; statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); statusType = b3GetStatusType(statusHandle); - if (statusType==CMD_STATE_LOGGING_START_COMPLETED) + if (statusType == CMD_STATE_LOGGING_START_COMPLETED) { loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle); } @@ -842,4 +853,3 @@ void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId) b3StateLoggingStop(commandHandle, stateLoggerUniqueId); statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle); } - diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index e6f658cc5..9fe1b8c50 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -9,23 +9,18 @@ #include - - - - struct b3RobotSimulatorLoadUrdfFileArgs { b3Vector3 m_startPosition; b3Quaternion m_startOrientation; bool m_forceOverrideFixedBase; - bool m_useMultiBody; + bool m_useMultiBody; b3RobotSimulatorLoadUrdfFileArgs() - : - m_startPosition(b3MakeVector3(0,0,0)), - m_startOrientation(b3Quaternion(0,0,0,1)), - m_forceOverrideFixedBase(false), - m_useMultiBody(true) + : m_startPosition(b3MakeVector3(0, 0, 0)), + m_startOrientation(b3Quaternion(0, 0, 0, 1)), + m_forceOverrideFixedBase(false), + m_useMultiBody(true) { } }; @@ -33,17 +28,15 @@ struct b3RobotSimulatorLoadUrdfFileArgs struct b3RobotSimulatorLoadSdfFileArgs { bool m_forceOverrideFixedBase; - bool m_useMultiBody; + bool m_useMultiBody; b3RobotSimulatorLoadSdfFileArgs() - : - m_forceOverrideFixedBase(false), - m_useMultiBody(true) + : m_forceOverrideFixedBase(false), + m_useMultiBody(true) { } }; - struct b3RobotSimulatorLoadFileResults { b3AlignedObjectArray m_uniqueObjectIds; @@ -55,7 +48,7 @@ struct b3RobotSimulatorLoadFileResults struct b3RobotSimulatorJointMotorArgs { int m_controlMode; - + double m_targetPosition; double m_kp; @@ -65,52 +58,52 @@ struct b3RobotSimulatorJointMotorArgs double m_maxTorqueValue; b3RobotSimulatorJointMotorArgs(int controlMode) - :m_controlMode(controlMode), - m_targetPosition(0), - m_kp(0.1), - m_targetVelocity(0), - m_kd(0.9), - m_maxTorqueValue(1000) + : m_controlMode(controlMode), + m_targetPosition(0), + m_kp(0.1), + m_targetVelocity(0), + m_kd(0.9), + m_maxTorqueValue(1000) { } }; enum b3RobotSimulatorInverseKinematicsFlags { - B3_HAS_IK_TARGET_ORIENTATION=1, - B3_HAS_NULL_SPACE_VELOCITY=2, - B3_HAS_JOINT_DAMPING=4, + B3_HAS_IK_TARGET_ORIENTATION = 1, + B3_HAS_NULL_SPACE_VELOCITY = 2, + B3_HAS_JOINT_DAMPING = 4, }; struct b3RobotSimulatorInverseKinematicArgs { int m_bodyUniqueId; -// double* m_currentJointPositions; -// int m_numPositions; + // double* m_currentJointPositions; + // int m_numPositions; double m_endEffectorTargetPosition[3]; double m_endEffectorTargetOrientation[4]; - int m_endEffectorLinkIndex; + int m_endEffectorLinkIndex; int m_flags; - int m_numDegreeOfFreedom; - b3AlignedObjectArray m_lowerLimits; - b3AlignedObjectArray m_upperLimits; - b3AlignedObjectArray m_jointRanges; - b3AlignedObjectArray m_restPoses; - b3AlignedObjectArray m_jointDamping; + int m_numDegreeOfFreedom; + b3AlignedObjectArray m_lowerLimits; + b3AlignedObjectArray m_upperLimits; + b3AlignedObjectArray m_jointRanges; + b3AlignedObjectArray m_restPoses; + b3AlignedObjectArray m_jointDamping; b3RobotSimulatorInverseKinematicArgs() - :m_bodyUniqueId(-1), - m_endEffectorLinkIndex(-1), - m_flags(0) + : m_bodyUniqueId(-1), + m_endEffectorLinkIndex(-1), + m_flags(0) { - m_endEffectorTargetPosition[0]=0; - m_endEffectorTargetPosition[1]=0; - m_endEffectorTargetPosition[2]=0; + m_endEffectorTargetPosition[0] = 0; + m_endEffectorTargetPosition[1] = 0; + m_endEffectorTargetPosition[2] = 0; - m_endEffectorTargetOrientation[0]=0; - m_endEffectorTargetOrientation[1]=0; - m_endEffectorTargetOrientation[2]=0; - m_endEffectorTargetOrientation[3]=1; + m_endEffectorTargetOrientation[0] = 0; + m_endEffectorTargetOrientation[1] = 0; + m_endEffectorTargetOrientation[2] = 0; + m_endEffectorTargetOrientation[3] = 1; } }; @@ -120,20 +113,19 @@ struct b3RobotSimulatorInverseKinematicsResults b3AlignedObjectArray m_calculatedJointPositions; }; - ///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet ///as documented in the pybullet Quickstart Guide ///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA class b3RobotSimulatorClientAPI { struct b3RobotSimulatorClientAPI_InternalData* m_data; - + public: b3RobotSimulatorClientAPI(); virtual ~b3RobotSimulatorClientAPI(); - bool connect(int mode, const std::string& hostName="localhost", int portOrKey=-1); - + bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1); + void disconnect(); bool isConnected() const; @@ -145,8 +137,8 @@ public: b3Quaternion getQuaternionFromEuler(const b3Vector3& rollPitchYaw); b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat); - int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args=b3RobotSimulatorLoadUrdfFileArgs()); - bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results=b3RobotSimulatorLoadFileResults(), const struct b3RobotSimulatorLoadSdfFileArgs& args=b3RobotSimulatorLoadSdfFileArgs()); + int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args = b3RobotSimulatorLoadUrdfFileArgs()); + bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results = b3RobotSimulatorLoadFileResults(), const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs()); bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); @@ -155,14 +147,17 @@ public: bool getBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation) const; bool resetBasePositionAndOrientation(int bodyUniqueId, b3Vector3& basePosition, b3Quaternion& baseOrientation); + bool getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const; + bool resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const; + int getNumJoints(int bodyUniqueId) const; bool getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo); - - void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); - bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state); - + void createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo); + + bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state); + bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue); void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args); @@ -170,29 +165,28 @@ public: void stepSimulation(); bool canSubmitCommand() const; - + void setRealTimeSimulation(bool enableRealTimeSimulation); void setGravity(const b3Vector3& gravityAcceleration); - + void setTimeStep(double timeStepInSeconds); - void setNumSimulationSubSteps(int numSubSteps); + void setNumSimulationSubSteps(int numSubSteps); void setNumSolverIterations(int numIterations); bool calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results); - bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian); - + bool getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian); + bool getLinkState(int bodyUniqueId, int linkIndex, b3LinkState* linkState); void configureDebugVisualizer(enum b3ConfigureDebugVisualizerEnum flag, int enable); - int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds); + int startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray& objectUniqueIds, int maxLogDof = -1); void stopStateLogging(int stateLoggerUniqueId); void getVREvents(b3VREventsData* vrEventsData); void getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData); - }; -#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H +#endif //B3_ROBOT_SIMULATOR_CLIENT_API_H diff --git a/src/Bullet3Common/b3Quaternion.h b/src/Bullet3Common/b3Quaternion.h index c89f2cf39..0b7564dcd 100644 --- a/src/Bullet3Common/b3Quaternion.h +++ b/src/Bullet3Common/b3Quaternion.h @@ -126,15 +126,16 @@ public: sinRoll * cosPitch * cosYaw - cosRoll * sinPitch * sinYaw, cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); } - /**@brief Set the quaternion using euler angles + + /**@brief Set the quaternion using euler angles * @param yaw Angle around Z * @param pitch Angle around Y * @param roll Angle around X */ - void setEulerZYX(const b3Scalar& yaw, const b3Scalar& pitch, const b3Scalar& roll) + void setEulerZYX(const b3Scalar& yawZ, const b3Scalar& pitchY, const b3Scalar& rollX) { - b3Scalar halfYaw = b3Scalar(yaw) * b3Scalar(0.5); - b3Scalar halfPitch = b3Scalar(pitch) * b3Scalar(0.5); - b3Scalar halfRoll = b3Scalar(roll) * b3Scalar(0.5); + b3Scalar halfYaw = b3Scalar(yawZ) * b3Scalar(0.5); + b3Scalar halfPitch = b3Scalar(pitchY) * b3Scalar(0.5); + b3Scalar halfRoll = b3Scalar(rollX) * b3Scalar(0.5); b3Scalar cosYaw = b3Cos(halfYaw); b3Scalar sinYaw = b3Sin(halfYaw); b3Scalar cosPitch = b3Cos(halfPitch); @@ -145,7 +146,30 @@ public: cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw, //y cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx + normalize(); } + + /**@brief Get the euler angles from this quaternion + * @param yaw Angle around Z + * @param pitch Angle around Y + * @param roll Angle around X */ + void getEulerZYX(b3Scalar& yawZ, b3Scalar& pitchY, b3Scalar& rollX) const + { + b3Scalar squ; + b3Scalar sqx; + b3Scalar sqy; + b3Scalar sqz; + b3Scalar sarg; + sqx = m_floats[0] * m_floats[0]; + sqy = m_floats[1] * m_floats[1]; + sqz = m_floats[2] * m_floats[2]; + squ = m_floats[3] * m_floats[3]; + rollX = b3Atan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz); + sarg = b3Scalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]); + pitchY = sarg <= b3Scalar(-1.0) ? b3Scalar(-0.5) * B3_PI: (sarg >= b3Scalar(1.0) ? b3Scalar(0.5) * B3_PI : b3Asin(sarg)); + yawZ = b3Atan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz); + } + /**@brief Add two quaternions * @param q The quaternion to add to this one */ B3_FORCE_INLINE b3Quaternion& operator+=(const b3Quaternion& q) From e0a4393bd21ed95f2fac180b8fbeb26ed709301a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 15 Mar 2017 17:35:10 -0700 Subject: [PATCH 4/4] fix compile issue --- examples/RobotSimulator/b3RobotSimulatorClientAPI.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h index 9fe1b8c50..a6f6ef1f9 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.h +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.h @@ -138,7 +138,7 @@ public: b3Vector3 getEulerFromQuaternion(const b3Quaternion& quat); int loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args = b3RobotSimulatorLoadUrdfFileArgs()); - bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results = b3RobotSimulatorLoadFileResults(), const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs()); + bool loadSDF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results, const struct b3RobotSimulatorLoadSdfFileArgs& args = b3RobotSimulatorLoadSdfFileArgs()); bool loadMJCF(const std::string& fileName, b3RobotSimulatorLoadFileResults& results); bool loadBullet(const std::string& fileName, b3RobotSimulatorLoadFileResults& results);