diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp index d4c708bccffdaa001ae9e9588a2d9728c72eb9b5..92c92045df132a9ee0d4a679d601476791a2b355 100644 --- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp +++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp @@ -181,8 +181,14 @@ void BulletObject::setPosition( const Eigen::Vector3f &posMM ) void BulletObject::setPoseIntern( const Eigen::Matrix4f &pose ) { - // notify motionState - motionState->setGlobalPose(pose); + /* convert to local coord system, apply comoffset and convert back*/ + Eigen::Matrix4f poseLocal = sceneObject->getGlobalPose().inverse() * pose; + poseLocal.block(0, 3, 3, 1) += com; + Eigen::Matrix4f poseGlobal = sceneObject->getGlobalPose() * poseLocal; + this->rigidBody->setWorldTransform(BulletEngine::getPoseBullet(poseGlobal)); + + // notify motionState -> not needed, automatically done + //motionState->setGlobalPose(pose); } void BulletObject::setPose( const Eigen::Matrix4f &pose ) diff --git a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp index 6858a1761b2e474dfeacf68b28ff3d65f3360d8c..c6623f99c3faefdeb1b429849e030a4ce5942b0d 100644 --- a/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp +++ b/SimDynamics/examples/SimDynamicsViewer/simDynamicsWindow.cpp @@ -63,7 +63,7 @@ SimDynamicsWindow::SimDynamicsWindow(std::string &sRobotFilename, Qt::WFlags fla o->setMass(1.0f); // 1kg dynamicsObject = dynamicsWorld->CreateDynamicsObject(o); - dynamicsObject->setPosition(Eigen::Vector3f(3000,3000,10000.0f)); + dynamicsObject->setPosition(Eigen::Vector3f(1000,2000,1000.0f)); dynamicsWorld->addObject(dynamicsObject); #if 0