Commit e54939f6 authored by Jennifer Buehler's avatar Jennifer Buehler
Browse files

Merge branch 'master' of github.com:JenniferBuehler/gazebo-pkgs

parents 1d500db7 86035653
...@@ -554,12 +554,14 @@ void GazeboGraspFix::OnUpdate() ...@@ -554,12 +554,14 @@ void GazeboGraspFix::OnUpdate()
// TODO: remove this test print, for issue #26 ------------------- // TODO: remove this test print, for issue #26 -------------------
#if 0 #if 0
physics::CollisionPtr objColl = boost::dynamic_pointer_cast<physics::Collision> physics::CollisionPtr objColl =
(world->GetEntity(objName)); boost::dynamic_pointer_cast<physics::Collision>(GetEntityByName(world, objName));
if (objColl && objColl->GetLink()) if (objColl && objColl->GetLink())
{ {
auto linVel = objColl->GetLink()->GetWorldLinearVel(); auto linVel = GetWorldVelocity(objColl->GetLink());
gzmsg << "Velocity for link " << objColl->GetLink()->GetName() << " (collision name " << objName << "): " << linVel << ", absolute val " << linVel.GetLength() << std::endl; gzmsg << "Velocity for link " << objColl->GetLink()->GetName()
<< " (collision name " << objName << "): " << linVel
<< ", absolute val " << GetLength(linVel) << std::endl;
} }
#endif #endif
// ------------------- // -------------------
......
...@@ -236,10 +236,10 @@ void GazeboGraspGripper::HandleDetach(const std::string &objName) ...@@ -236,10 +236,10 @@ void GazeboGraspGripper::HandleDetach(const std::string &objName)
#if 0 #if 0
if (obj && obj->GetLink()) if (obj && obj->GetLink())
{ {
auto linVel = obj->GetLink()->GetWorldLinearVel(); auto linVel = GetWorldVelocity(obj->GetLink());
gzmsg << "PRE-DETACH Velocity for link " << obj->GetLink()->GetName() gzmsg << "PRE-DETACH Velocity for link " << obj->GetLink()->GetName()
<< " (collision name " << objName << "): " << " (collision name " << objName << "): " << linVel
<< linVel << ", absolute val " << linVel.GetLength() << std::endl; << ", absolute val " << GetLength(linVel) << std::endl;
} }
#endif #endif
// ------------------- // -------------------
...@@ -250,10 +250,11 @@ void GazeboGraspGripper::HandleDetach(const std::string &objName) ...@@ -250,10 +250,11 @@ void GazeboGraspGripper::HandleDetach(const std::string &objName)
#if 0 #if 0
if (obj && obj->GetLink()) if (obj && obj->GetLink())
{ {
auto linVel = obj->GetLink()->GetWorldLinearVel(); auto linVel = GetWorldVelocity(obj->GetLink());
gzmsg << "POST-DETACH Velocity for link " << obj->GetLink()->GetName() gzmsg << "POST-DETACH Velocity for link " << obj->GetLink()->GetName()
<< " (collision name " << objName << "): " << " (collision name " << objName << "): " << linVel
<< linVel << ", absolute val " << linVel.GetLength() << std::endl; << ", absolute val " << GetLength(linVel) << std::endl;
} }
#endif #endif
// ------------------- // -------------------
......
...@@ -29,6 +29,9 @@ typedef gz_math::Matrix3 GzMatrix3; ...@@ -29,6 +29,9 @@ typedef gz_math::Matrix3 GzMatrix3;
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
GzPose3 GetWorldPose(const gazebo::physics::LinkPtr &link); GzPose3 GetWorldPose(const gazebo::physics::LinkPtr &link);
///////////////////////////////////////////////////////////////////////////////
GzVector3 GetWorldVelocity(const gazebo::physics::LinkPtr &link);
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
GzMatrix4 GetIdentity(); GzMatrix4 GetIdentity();
......
...@@ -16,6 +16,17 @@ GzPose3 gazebo::GetWorldPose(const gazebo::physics::LinkPtr &link) ...@@ -16,6 +16,17 @@ GzPose3 gazebo::GetWorldPose(const gazebo::physics::LinkPtr &link)
#endif #endif
} }
///////////////////////////////////////////////////////////////////////////////
GzVector3 gazebo::GetWorldVelocity(const gazebo::physics::LinkPtr &link)
{
#if GAZEBO_MAJOR_VERSION >= 8
return link->WorldLinearVel();
#else
return link->GetWorldLinearVel();
#endif
}
/////////////////////////////////////////////////////////////////////////////// ///////////////////////////////////////////////////////////////////////////////
GzMatrix4 gazebo::GetIdentity() GzMatrix4 gazebo::GetIdentity()
{ {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment