From 88aa9e899ef35703d49d320133a39196afd188d3 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 24 Feb 2017 21:40:43 -0800 Subject: [PATCH 1/2] 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); From 0131754173f38d63659c9d8c57ce2d1fb03cf51c Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Sat, 25 Feb 2017 16:57:18 -0800 Subject: [PATCH 2/2] Add 'extractRotation' based on "A robust method to extract the rotational part of deformations" ///See http://dl.acm.org/citation.cfm?doid=2994258.2994269 Rewrite 'diagonalize' to use 'extractRotation', should fix Issue 846 --- src/LinearMath/btMatrix3x3.h | 116 +++++++++++----------------------- src/LinearMath/btQuaternion.h | 30 +++++++-- 2 files changed, 62 insertions(+), 84 deletions(-) diff --git a/src/LinearMath/btMatrix3x3.h b/src/LinearMath/btMatrix3x3.h index 40cd1e086..96d0ba98f 100644 --- a/src/LinearMath/btMatrix3x3.h +++ b/src/LinearMath/btMatrix3x3.h @@ -647,92 +647,48 @@ public: return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); } + ///extractRotation is from "A robust method to extract the rotational part of deformations" + ///See http://dl.acm.org/citation.cfm?doid=2994258.2994269 + SIMD_FORCE_INLINE void extractRotation(btQuaternion &q,btScalar tolerance = 1.0e-9, int maxIter=100) + { + int iter =0; + btScalar w; + const btMatrix3x3& A=*this; + for(iter = 0; iter < maxIter; iter++) + { + btMatrix3x3 R(q); + btVector3 omega = (R.getColumn(0).cross(A.getColumn(0)) + R.getColumn(1).cross(A.getColumn(1)) + + R.getColumn(2).cross(A.getColumn(2)) + ) * (btScalar(1.0) / btFabs(R.getColumn(0).dot(A.getColumn(0)) + R.getColumn + (1).dot(A.getColumn(1)) + R.getColumn(2).dot(A.getColumn(2))) + + tolerance); + w = omega.norm(); + if(w < tolerance) + break; + q = btQuaternion(btVector3((btScalar(1.0) / w) * omega),w) * + q; + q.normalize(); + } + } - /**@brief diagonalizes this matrix by the Jacobi method. + + + /**@brief diagonalizes this matrix * @param rot stores the rotation from the coordinate system in which the matrix is diagonal to the original * coordinate system, i.e., old_this = rot * new_this * rot^T. * @param threshold See iteration - * @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied - * by the sum of the absolute values of the diagonal, or when maxSteps have been executed. - * - * Note that this matrix is assumed to be symmetric. + * @param maxIter The iteration stops when we hit the given tolerance or when maxIter have been executed. */ - void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) + void diagonalize(btMatrix3x3& rot, btScalar tolerance = 1.0e-9, int maxIter=100) { - rot.setIdentity(); - for (int step = maxSteps; step > 0; step--) - { - // find off-diagonal element [p][q] with largest magnitude - int p = 0; - int q = 1; - int r = 2; - btScalar max = btFabs(m_el[0][1]); - btScalar v = btFabs(m_el[0][2]); - if (v > max) - { - q = 2; - r = 1; - max = v; - } - v = btFabs(m_el[1][2]); - if (v > max) - { - p = 1; - q = 2; - r = 0; - max = v; - } - - btScalar t = threshold * (btFabs(m_el[0][0]) + btFabs(m_el[1][1]) + btFabs(m_el[2][2])); - if (max <= t) - { - if (max <= SIMD_EPSILON * t) - { - return; - } - step = 1; - } - - // compute Jacobi rotation J which leads to a zero for element [p][q] - btScalar mpq = m_el[p][q]; - btScalar theta = (m_el[q][q] - m_el[p][p]) / (2 * mpq); - btScalar theta2 = theta * theta; - btScalar cos; - btScalar sin; - if (theta2 * theta2 < btScalar(10 / SIMD_EPSILON)) - { - t = (theta >= 0) ? 1 / (theta + btSqrt(1 + theta2)) - : 1 / (theta - btSqrt(1 + theta2)); - cos = 1 / btSqrt(1 + t * t); - sin = cos * t; - } - else - { - // approximation for large theta-value, i.e., a nearly diagonal matrix - t = 1 / (theta * (2 + btScalar(0.5) / theta2)); - cos = 1 - btScalar(0.5) * t * t; - sin = cos * t; - } - - // apply rotation to matrix (this = J^T * this * J) - m_el[p][q] = m_el[q][p] = 0; - m_el[p][p] -= t * mpq; - m_el[q][q] += t * mpq; - btScalar mrp = m_el[r][p]; - btScalar mrq = m_el[r][q]; - m_el[r][p] = m_el[p][r] = cos * mrp - sin * mrq; - m_el[r][q] = m_el[q][r] = cos * mrq + sin * mrp; - - // apply rotation to rot (rot = rot * J) - for (int i = 0; i < 3; i++) - { - btVector3& row = rot[i]; - mrp = row[p]; - mrq = row[q]; - row[p] = cos * mrp - sin * mrq; - row[q] = cos * mrq + sin * mrp; - } - } + btQuaternion r; + extractRotation(r,tolerance,maxIter); + rot.setRotation(r); + btMatrix3x3 rotInv = btMatrix3x3(r.inverse()); + btMatrix3x3 old = *this; + setValue(old.tdotx( rotInv[0]), old.tdoty( rotInv[0]), old.tdotz( rotInv[0]), + old.tdotx( rotInv[1]), old.tdoty( rotInv[1]), old.tdotz( rotInv[1]), + old.tdotx( rotInv[2]), old.tdoty( rotInv[2]), old.tdotz( rotInv[2])); } diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index 32f0f85d2..a9b78022e 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -141,11 +141,11 @@ public: * @param yaw Angle around Z * @param pitch Angle around Y * @param roll Angle around X */ - void setEulerZYX(const btScalar& yaw, const btScalar& pitch, const btScalar& roll) + void setEulerZYX(const btScalar& yawZ, const btScalar& pitchY, const btScalar& rollX) { - btScalar halfYaw = btScalar(yaw) * btScalar(0.5); - btScalar halfPitch = btScalar(pitch) * btScalar(0.5); - btScalar halfRoll = btScalar(roll) * btScalar(0.5); + btScalar halfYaw = btScalar(yawZ) * btScalar(0.5); + btScalar halfPitch = btScalar(pitchY) * btScalar(0.5); + btScalar halfRoll = btScalar(rollX) * btScalar(0.5); btScalar cosYaw = btCos(halfYaw); btScalar sinYaw = btSin(halfYaw); btScalar cosPitch = btCos(halfPitch); @@ -157,6 +157,28 @@ public: cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx } + + /**@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(btScalar& yawZ, btScalar& pitchY, btScalar& rollX) const + { + btScalar squ; + btScalar sqx; + btScalar sqy; + btScalar sqz; + btScalar 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 = btAtan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz); + sarg = btScalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]); + pitchY = sarg <= btScalar(-1.0) ? btScalar(-0.5) * SIMD_PI: (sarg >= btScalar(1.0) ? btScalar(0.5) * SIMD_PI : btAsin(sarg)); + yawZ = btAtan2(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 */ SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q)