Implement Link::SetLinearVel and Link::SetAngularVel for simbody (#1080)

gazebosim · Jul 31, 2014 · 4d9dc31 · 4d9dc31
1 parent 2f64462
commit 4d9dc31
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 8 deletions.
diff --git a/gazebo/physics/simbody/SimbodyLink.cc b/gazebo/physics/simbody/SimbodyLink.cc
@@ -330,8 +330,12 @@ void SimbodyLink::SetEnabled(bool /*_enable*/) const
 }
 
 //////////////////////////////////////////////////
-void SimbodyLink::SetLinearVel(const math::Vector3 & /*_vel*/)
+void SimbodyLink::SetLinearVel(const math::Vector3 & _vel)
 {
+  this->masterMobod.setUToFitLinearVelocity(
+    this->simbodyPhysics->integ->updAdvancedState(),
+    //this->simbodyPhysics->integ->getState(),
+    SimbodyPhysics::Vector3ToVec3(_vel));
 }
 
 //////////////////////////////////////////////////
@@ -412,8 +416,12 @@ math::Vector3 SimbodyLink::GetWorldCoGLinearVel() const
 }
 
 //////////////////////////////////////////////////
-void SimbodyLink::SetAngularVel(const math::Vector3 &/*_vel*/)
+void SimbodyLink::SetAngularVel(const math::Vector3 &_vel)
 {
+  this->masterMobod.setUToFitAngularVelocity(
+    this->simbodyPhysics->integ->updAdvancedState(),
+    //this->simbodyPhysics->integ->getState(),
+    SimbodyPhysics::Vector3ToVec3(_vel));
 }
 
 //////////////////////////////////////////////////

diff --git a/test/integration/physics_link.cc b/test/integration/physics_link.cc
@@ -81,12 +81,6 @@ void PhysicsLinkTest::GetWorldEnergy(const std::string &_physicsEngine)
 /////////////////////////////////////////////////
 void PhysicsLinkTest::SetVelocity(const std::string &_physicsEngine)
 {
-  if (_physicsEngine == "simbody")
-  {
-    gzerr << "SimbodyLink::SetLinearVel, SetAngularVel aren't working (#1080)"
-          << std::endl;
-    return;
-  }
   if (_physicsEngine == "dart")
   {
     gzerr << "DARTLink::SetLinearVel, SetAngularVel aren't working (#1079)"