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

Fixes for test prints (now also requiring version dependent gz calls)

parent dcd61103
......@@ -554,12 +554,14 @@ void GazeboGraspFix::OnUpdate()
// TODO: remove this test print, for issue #26 -------------------
#if 0
physics::CollisionPtr objColl = boost::dynamic_pointer_cast<physics::Collision>
(world->GetEntity(objName));
physics::CollisionPtr objColl =
boost::dynamic_pointer_cast<physics::Collision>(GetEntityByName(world, objName));
if (objColl && objColl->GetLink())
{
auto linVel = objColl->GetLink()->GetWorldLinearVel();
gzmsg << "Velocity for link " << objColl->GetLink()->GetName() << " (collision name " << objName << "): " << linVel << ", absolute val " << linVel.GetLength() << std::endl;
auto linVel = GetWorldVelocity(objColl->GetLink());
gzmsg << "Velocity for link " << objColl->GetLink()->GetName()
<< " (collision name " << objName << "): " << linVel
<< ", absolute val " << GetLength(linVel) << std::endl;
}
#endif
// -------------------
......
......@@ -236,10 +236,10 @@ void GazeboGraspGripper::HandleDetach(const std::string &objName)
#if 0
if (obj && obj->GetLink())
{
auto linVel = obj->GetLink()->GetWorldLinearVel();
auto linVel = GetWorldVelocity(obj->GetLink());
gzmsg << "PRE-DETACH Velocity for link " << obj->GetLink()->GetName()
<< " (collision name " << objName << "): "
<< linVel << ", absolute val " << linVel.GetLength() << std::endl;
<< " (collision name " << objName << "): " << linVel
<< ", absolute val " << GetLength(linVel) << std::endl;
}
#endif
// -------------------
......@@ -250,10 +250,11 @@ void GazeboGraspGripper::HandleDetach(const std::string &objName)
#if 0
if (obj && obj->GetLink())
{
auto linVel = obj->GetLink()->GetWorldLinearVel();
gzmsg << "POST-DETACH Velocity for link " << obj->GetLink()->GetName()
<< " (collision name " << objName << "): "
<< linVel << ", absolute val " << linVel.GetLength() << std::endl;
auto linVel = GetWorldVelocity(obj->GetLink());
gzmsg << "PRE-DETACH Velocity for link " << obj->GetLink()->GetName()
<< " (collision name " << objName << "): " << linVel
<< ", absolute val " << GetLength(linVel) << std::endl;
}
#endif
// -------------------
......
......@@ -29,6 +29,9 @@ typedef gz_math::Matrix3 GzMatrix3;
///////////////////////////////////////////////////////////////////////////////
GzPose3 GetWorldPose(const gazebo::physics::LinkPtr &link);
///////////////////////////////////////////////////////////////////////////////
GzVector3 GetWorldVelocity(const gazebo::physics::LinkPtr &link);
///////////////////////////////////////////////////////////////////////////////
GzMatrix4 GetIdentity();
......
......@@ -16,6 +16,17 @@ GzPose3 gazebo::GetWorldPose(const gazebo::physics::LinkPtr &link)
#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()
{
......
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