diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 89260d47d..13bd8a2ad 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -82,6 +82,12 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], btQuaternion deltaQ = endQ*startQ.inverse(); float angle = deltaQ.getAngle(); btVector3 axis = deltaQ.getAxis(); + if (angle > PI) { + angle -= 2.0*PI; + } + else if (angle < -PI) { + angle += 2.0*PI; + } float angleDot = angle; btVector3 angularVel = angleDot*axis.normalize(); for (int i = 0; i < 3; ++i) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f0fa8359c..4209631e2 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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()); 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));