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

Fix the desired angular velocity in IK to prevent flip motion.

Browse Source
This commit is contained in:
yunfeibai
2016-10-05 14:24:49 -07:00
parent ae5844d4fb
commit 0d58ebb311
2 changed files with 6 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -82,6 +82,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
btQuaternion deltaQ = endQ*startQ.inverse(); btQuaternion deltaQ = endQ*startQ.inverse();
float angle = deltaQ.getAngle(); float angle = deltaQ.getAngle();
btVector3 axis = deltaQ.getAxis(); btVector3 axis = deltaQ.getAxis();
if (angle > PI) {
angle -= 2.0*PI;
}
else if (angle < -PI) {
angle += 2.0*PI;
}
float angleDot = angle; float angleDot = angle;
btVector3 angularVel = angleDot*axis.normalize(); btVector3 angularVel = angleDot*axis.normalize();
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)

1
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3136,7 +3136,6 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); loadUrdf("husky/husky.urdf", btVector3(2, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_huskyId = bodyId; m_data->m_huskyId = bodyId;
//loadUrdf("jz/jz.urdf", btVector3(0, -5, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size());
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10)); m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, -10));