[Fix]: allow reset the base of the agents

Changes: * Fixed a small issue with the free joints, as they should reset the base instead of setting the q-values directly (qpos does not exist for this joint as it is fixed)
wpumacay · Aug 20, 2019 · 4916faf · 4916faf
1 parent 67da29c
commit 4916faf
Showing 1 changed file with 10 additions and 6 deletions.
diff --git a/src/bullet_agent_wrapper.cpp b/src/bullet_agent_wrapper.cpp
@@ -771,14 +771,18 @@ namespace bullet {
                 if ( _jointPtr->type == "free" )
                 {
                     auto _position = m_agentPtr->getStartPosition();
-                    auto _rotation = TMat3::toQuaternion( TMat3::fromEuler( m_agentPtr->getStartRotation() ) );
+                    auto _rotEuler = m_agentPtr->getStartRotation();
+                    auto _transform = tysoc::TMat4( _position, _rotEuler );
 
-                    // use the start position ( x - y - z - qw - qx - qy - qz )
-                    _qposs = { _position.x, _position.y, _position.z,
-                               _rotation.w, _rotation.x, _rotation.y, _rotation.z };
+                    // for this case, we must use the base directly :(
+                    m_btMultiBodyPtr->setBasePos( utils::toBtVec3( _position ) );
+                    m_btMultiBodyPtr->setBaseWorldTransform( utils::toBtTransform( _transform ) );
 
-                    // and no initial velocities
-                    _qvels = { 0., 0., 0., 0., 0., 0. };
+                    m_btMultiBodyPtr->setBaseVel( utils::toBtVec3( { 0., 0., 0. } ) );
+                    m_btMultiBodyPtr->setBaseOmega( utils::toBtVec3( { 0., 0., 0. } ) );
+
+                    // skip setting the q-values for this joint
+                    continue;
                 }
                 else if ( _jointPtr->type == "slider" || _jointPtr->type == "slide" || _jointPtr->type == "prismatic" )
                 {