From ed1d46a358c5b1863d105b5394702516a7984ab5 Mon Sep 17 00:00:00 2001
From: vahrenkamp <vahrenkamp@042f3d55-54a8-47e9-b7fb-15903f145c44>
Date: Wed, 30 Apr 2014 12:34:42 +0000
Subject: [PATCH] Fixed setPose for dynamics objects
git-svn-id: http://svn.code.sf.net/p/simox/code/trunk@565 042f3d55-54a8-47e9-b7fb-15903f145c44
---
.../DynamicsEngine/BulletEngine/BulletObject.cpp | 10 ++++++++--
.../examples/SimDynamicsViewer/simDynamicsWindow.cpp | 2 +-
2 files changed, 9 insertions(+), 3 deletions(-)
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index d4c708bcc..92c92045d 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 6858a1761..c6623f99c 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
--
GitLab