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

Merge pull request #975 from erwincoumans/master

Browse Source 
Rewrite 'diagonalize' to use 'extractRotation', should fix Issue 846
///See http://dl.acm.org/citation.cfm?doid=2994258.2994269
tweak Minitaur/RobotSimulator, fix target value from int to double.
This commit is contained in:
erwincoumans
2017-02-26 07:01:42 -08:00
committed by GitHub
parent 77538b04c2 0131754173
commit e2bdf124a1
7 changed files with 140 additions and 129 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
examples/RobotSimulator/MinitaurSetup.cpp
View File

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

3
examples/RobotSimulator/MinitaurSetup.h
View File

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

8
examples/RobotSimulator/RobotSimulatorMain.cpp
View File

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

2
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

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

2
examples/RobotSimulator/b3RobotSimulatorClientAPI.h
View File

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

116
src/LinearMath/btMatrix3x3.h
View File

@@ -647,92 +647,48 @@ public:
return m_el[0].z() * v.x() + m_el[1].z() * v.y() + m_el[2].z() * v.z(); 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 * @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. * coordinate system, i.e., old_this = rot * new_this * rot^T.
* @param threshold See iteration * @param threshold See iteration
* @param iteration The iteration stops when all off-diagonal elements are less than the threshold multiplied * @param maxIter The iteration stops when we hit the given tolerance or when maxIter have been executed.
* 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.
*/ */
void diagonalize(btMatrix3x3& rot, btScalar threshold, int maxSteps) void diagonalize(btMatrix3x3& rot, btScalar tolerance = 1.0e-9, int maxIter=100)
{ {
rot.setIdentity(); btQuaternion r;
for (int step = maxSteps; step > 0; step--) extractRotation(r,tolerance,maxIter);
{ rot.setRotation(r);
// find off-diagonal element [p][q] with largest magnitude btMatrix3x3 rotInv = btMatrix3x3(r.inverse());
int p = 0; btMatrix3x3 old = *this;
int q = 1; setValue(old.tdotx( rotInv[0]), old.tdoty( rotInv[0]), old.tdotz( rotInv[0]),
int r = 2; old.tdotx( rotInv[1]), old.tdoty( rotInv[1]), old.tdotz( rotInv[1]),
btScalar max = btFabs(m_el[0][1]); old.tdotx( rotInv[2]), old.tdoty( rotInv[2]), old.tdotz( rotInv[2]));
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;
}
}
} }

30
src/LinearMath/btQuaternion.h
View File

@@ -141,11 +141,11 @@ public:
* @param yaw Angle around Z * @param yaw Angle around Z
* @param pitch Angle around Y * @param pitch Angle around Y
* @param roll Angle around X */ * @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 halfYaw = btScalar(yawZ) * btScalar(0.5);
btScalar halfPitch = btScalar(pitch) * btScalar(0.5); btScalar halfPitch = btScalar(pitchY) * btScalar(0.5);
btScalar halfRoll = btScalar(roll) * btScalar(0.5); btScalar halfRoll = btScalar(rollX) * btScalar(0.5);
btScalar cosYaw = btCos(halfYaw); btScalar cosYaw = btCos(halfYaw);
btScalar sinYaw = btSin(halfYaw); btScalar sinYaw = btSin(halfYaw);
btScalar cosPitch = btCos(halfPitch); btScalar cosPitch = btCos(halfPitch);
@@ -157,6 +157,28 @@ public:
cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw, //z
cosRoll * cosPitch * cosYaw + sinRoll * sinPitch * sinYaw); //formerly yzx 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 /**@brief Add two quaternions
* @param q The quaternion to add to this one */ * @param q The quaternion to add to this one */
SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q) SIMD_FORCE_INLINE btQuaternion& operator+=(const btQuaternion& q)