void fillUrdfJoint(const rbd::Joint& rbdJ, const Limits& limits, urdf::Joint& urdfJ)
{
switch(rbdJ.type())
{
case rbd::Joint::Fixed:
urdfJ.type = urdf::Joint::FIXED;
break;
case rbd::Joint::Rev:
if(exist(limits.ql, rbdJ.id()) && exist(limits.qu, rbdJ.id()))
{
urdfJ.type = urdf::Joint::REVOLUTE;
}
else
{
urdfJ.type = urdf::Joint::CONTINUOUS;
}
urdfJ.axis = fromEigen(Eigen::Vector3d(rbdJ.motionSubspace().block<3,1>(0,0)));
break;
case rbd::Joint::Prism:
urdfJ.type = urdf::Joint::PRISMATIC;
urdfJ.axis = fromEigen(Eigen::Vector3d(rbdJ.motionSubspace().block<3,1>(3,0)));
break;
case rbd::Joint::Planar:
urdfJ.type = urdf::Joint::PLANAR;
urdfJ.axis = fromEigen(Eigen::Vector3d(Eigen::Vector3d::UnitZ()));
break;
case rbd::Joint::Free:
urdfJ.type = urdf::Joint::FLOATING;
break;
case rbd::Joint::Spherical:
throw std::runtime_error("Impossible to export spherical joint to urdf");
default:
throw std::runtime_error("Unknow RBDyn joint type");
}
}
urdf::Pose transformToPose(const sva::PTransformd& X)
{
urdf::Pose pose;
pose.position = fromEigen(X.translation());
pose.rotation = fromEigen(Eigen::Matrix3d(X.rotation().inverse()));
return pose;
}
void
Tracker::broadcast_tf()
{
bool val;
config->get("broadcast_tf", val);
if (!val || !started)
return;
std::string frame;
storage->readSensorFrame(frame);
tf::Transform tran;
if(transform){
fromEigen(*transform, tran);
tf_brc.sendTransform(tf::StampedTransform(tran, ros::Time::now(), frame.c_str(), obj_name.first.c_str()));
}
}
void
Tracker::update_markers()
{
if (!marks)
return;
geometry_msgs::Pose pose;
if(transform){
bool val;
config->get("publish_bounding_box", val);
fromEigen(*transform, pose);
marks->markers.resize(1);
if (val)
create_bb_marker(pose);
for (auto& x: marks->markers)
x.pose = pose;
}
}
void
Tracker::create_markers()
{
std::string ref_frame;
storage->readSensorFrame(ref_frame);
marks = std::make_shared<visualization_msgs::MarkerArray>();
geometry_msgs::Pose pose;
fromEigen(*transform, pose);
visualization_msgs::Marker obj_mark;
create_object_marker(pose, obj_name, obj_mark);
obj_mark.header.frame_id = ref_frame.c_str();
obj_mark.color.r = 1.0f;
obj_mark.color.g = 0.0f;
obj_mark.color.b = 0.5f;
obj_mark.ns = obj_name.first;
obj_mark.id = 0;
marks->markers.push_back(obj_mark);
}
urdf::Rotation fromEigen(const Eigen::Matrix3d& rot)
{
return fromEigen(Eigen::Quaterniond(rot));
}
