From bec1b8ef67f40b544bb82b2f9d00e512b8363f07 Mon Sep 17 00:00:00 2001
From: Adrian Knobloch <adrian.knobloch@student.kit.edu>
Date: Mon, 26 Jun 2017 21:21:45 +0200
Subject: [PATCH] Fix wrong initial positioning of robot nodes
---
SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp | 9 +++++----
1 file changed, 5 insertions(+), 4 deletions(-)
diff --git a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
index 680bb732d..d78cbbd11 100644
--- a/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
+++ b/SimDynamics/DynamicsEngine/BulletEngine/BulletObject.cpp
@@ -281,9 +281,10 @@ namespace SimDynamics
{
MutexLockPtr lock = getScopedLock();
/* convert to local coord system, apply comoffset and convert back*/
- Eigen::Matrix4f poseCom = pose;
- poseCom.block(0, 3, 3, 1) += com;
- this->rigidBody->setWorldTransform(BulletEngine::getPoseBullet(poseCom));
+ Eigen::Matrix4f poseLocal = Eigen::Matrix4f::Identity();
+ poseLocal.block(0, 3, 3, 1) += com;
+ Eigen::Matrix4f poseGlobal = pose * poseLocal;
+ this->rigidBody->setWorldTransform(BulletEngine::getPoseBullet(poseGlobal));
// notify motionState of non-robot nodes
if(!boost::dynamic_pointer_cast<VirtualRobot::RobotNode>(sceneObject))
@@ -295,7 +296,7 @@ namespace SimDynamics
void BulletObject::setPose(const Eigen::Matrix4f& pose)
{
MutexLockPtr lock = getScopedLock();
- //DynamicsObject::setPose(pose);
+ DynamicsObject::setPose(pose);
setPoseIntern(pose);
}
--
GitLab