From 4d9dc313d22a719f3d4c7608e179aab4f43f7510 Mon Sep 17 00:00:00 2001
From: Steve Peters <scpeters@osrfoundation.org>
Date: Thu, 31 Jul 2014 03:01:05 -0700
Subject: [PATCH] Implement Link::SetLinearVel and Link::SetAngularVel for
 simbody (#1080)

---
 gazebo/physics/simbody/SimbodyLink.cc | 12 ++++++++++--
 test/integration/physics_link.cc      |  6 ------
 2 files changed, 10 insertions(+), 8 deletions(-)

diff --git a/gazebo/physics/simbody/SimbodyLink.cc b/gazebo/physics/simbody/SimbodyLink.cc
index 14ba1a54c8..6e2d6945ed 100644
--- 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
index a57ceec58d..c58daf97b3 100644
--- 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)"