bool URDF_RBDL_Model::initFrom(const urdf::Model& model, const std::string& root)
{
// Reset root mass to zero and give it the proper name
mBodies[0].mMass = 0;
// Set gravity to ROS standards
gravity << 0, 0, -9.81;
std::vector<boost::shared_ptr<urdf::Link> > links;
model.getLinks(links);
boost::shared_ptr<const urdf::Link> oldRoot = model.getRoot();
boost::shared_ptr<const urdf::Link> newRoot = model.getLink(root);
if(oldRoot == newRoot || !newRoot)
process(*oldRoot, 0, Math::SpatialTransform());
else
processReverse(*newRoot, 0, 0, Math::SpatialTransform());
// Rename the root body to our URDF root
mBodyNameMap.erase("ROOT");
mBodyNameMap[root] = 0;
return true;
}
bool getChainInfoFromRobotModel(urdf::Model &robot_model,
const std::string &root_name,
const std::string &tip_name,
kinematics_msgs::KinematicSolverInfo &chain_info)
{
// get joint maxs and mins
boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint;
while (link && link->name != root_name)
{
joint = robot_model.getJoint(link->parent_joint->name);
if (!joint)
{
ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
return false;
}
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
{
float lower, upper;
int hasLimits;
if ( joint->type != urdf::Joint::CONTINUOUS )
{
lower = joint->limits->lower;
upper = joint->limits->upper;
hasLimits = 1;
}
else
{
lower = -M_PI;
upper = M_PI;
hasLimits = 0;
}
chain_info.joint_names.push_back(joint->name);
motion_planning_msgs::JointLimits limits;
limits.joint_name = joint->name;
limits.has_position_limits = hasLimits;
limits.min_position = lower;
limits.max_position = upper;
chain_info.limits.push_back(limits);
}
link = robot_model.getLink(link->getParent()->name);
}
link = robot_model.getLink(tip_name);
if(link)
chain_info.link_names.push_back(tip_name);
std::reverse(chain_info.limits.begin(),chain_info.limits.end());
std::reverse(chain_info.joint_names.begin(),chain_info.joint_names.end());
return true;
}
void makeJointMarker(std::string jointName)
{
boost::shared_ptr<const urdf::Joint> targetJoint = huboModel.getJoint(jointName);
// The marker must be created in the parent frame so you don't get feedback when you move it
visualization_msgs::InteractiveMarker marker;
marker.scale = .125;
marker.name = jointName;
marker.header.frame_id = targetJoint->parent_link_name;
geometry_msgs::Pose controlPose = hubo_motion_ros::toPose( targetJoint->parent_to_joint_origin_transform);
marker.pose = controlPose;
visualization_msgs::InteractiveMarkerControl control;
Eigen::Quaternionf jointAxis;
Eigen::Vector3f axisVector = hubo_motion_ros::toEVector3(targetJoint->axis);
jointAxis.setFromTwoVectors(Eigen::Vector3f::UnitX(), axisVector);
control.orientation.w = jointAxis.w();
control.orientation.x = jointAxis.x();
control.orientation.y = jointAxis.y();
control.orientation.z = jointAxis.z();
control.always_visible = true;
control.interaction_mode = visualization_msgs::InteractiveMarkerControl::ROTATE_AXIS;
control.orientation_mode = visualization_msgs::InteractiveMarkerControl::INHERIT;
marker.controls.push_back(control);
gIntServer->insert(marker);
gIntServer->setCallback(marker.name, &processFeedback);
}
// Get Joint Limits from an URDF model
void getJointLimitsFromModel(const urdf::Model& model,
std::vector<double> &q_min,
std::vector<double> &q_max,
KDL::Chain &kdlArmChain
)
{
boost::shared_ptr<const urdf::Joint> joint;
for(unsigned int i=0; i< kdlArmChain.getNrOfJoints(); ++i)
{
if (kdlArmChain.getSegment(i).getJoint().getType() != KDL::Joint::None)
{
joint = model.getJoint(kdlArmChain.getSegment(i).getJoint().getName());
if ( joint->type != urdf::Joint::CONTINUOUS )
{
q_min.at(i) = joint->limits->lower;
q_max.at(i) = joint->limits->upper;
}
else
{
q_min.at(i) = -3.14/2.0;
q_max.at(i) = 3.14/2.0;
}
}
}
}
bool loadRobotModel(ros::NodeHandle node_handle, urdf::Model &robot_model, std::string &root_name, std::string &tip_name, std::string &xml_string)
{
std::string urdf_xml,full_urdf_xml;
node_handle.param("urdf_xml",urdf_xml,std::string("robot_description"));
node_handle.searchParam(urdf_xml,full_urdf_xml);
TiXmlDocument xml;
ROS_DEBUG("Reading xml file from parameter server\n");
std::string result;
if (node_handle.getParam(full_urdf_xml, result))
xml.Parse(result.c_str());
else
{
ROS_FATAL("Could not load the xml from parameter server: %s\n", urdf_xml.c_str());
return false;
}
xml_string = result;
TiXmlElement *root_element = xml.RootElement();
TiXmlElement *root = xml.FirstChildElement("robot");
if (!root || !root_element)
{
ROS_FATAL("Could not parse the xml from %s\n", urdf_xml.c_str());
exit(1);
}
robot_model.initXml(root);
if (!node_handle.getParam("root_name", root_name)){
ROS_FATAL("No root name found on parameter server");
return false;
}
if (!node_handle.getParam("tip_name", tip_name)){
ROS_FATAL("No tip name found on parameter server");
return false;
}
return true;
}
/* ------------------------ KinematicModel ------------------------ */
planning_models::KinematicModel::KinematicModel(const urdf::Model &model,
const std::vector<GroupConfig>& group_configs,
const std::vector<MultiDofConfig>& multi_dof_configs)
{
model_name_ = model.getName();
if (model.getRoot())
{
const urdf::Link *root = model.getRoot().get();
root_ = buildRecursive(NULL, root, multi_dof_configs);
buildGroups(group_configs);
}
else
{
root_ = NULL;
ROS_WARN("No root link found");
}
}
bool FkLookup::addRoot(const urdf::Model &model, const std::string &root){
boost::shared_ptr<KDL::Tree> tree;
tree_map::iterator it = roots.find(root);
if(it != roots.end()){
ROS_WARN_STREAM("Link " << root << " already processed");
return false;
}
if(!model.getLink(root)){
ROS_ERROR_STREAM("Model does not include link '" << root << "'");
return false;
}
tree.reset(new KDL::Tree());
kdl_parser::treeFromUrdfModel(model, *tree);
roots.insert(std::make_pair(root,tree));
crawl_and_add_links(model.getLink(root)->child_links,root,tips);
return !tips.empty();
}
SolverInfoProcessor::SolverInfoProcessor(const urdf::Model &robot_model,
const std::string &tip_name,
const std::string &root_name)
{
// setup the IK solver information which contains joint names, joint limits and so on
boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
while (link) {
// check if we have already reached the final link
if (link->name == root_name) break;
// if we have reached the last joint the root frame is not in the chain
// then we cannot build the chain
if (!link->parent_joint) {
ROS_ERROR("The provided root link is not in the chain");
ROS_ASSERT(false);
}
// process the joint
boost::shared_ptr<const urdf::Joint> joint = robot_model.getJoint(link->parent_joint->name);
if (!joint) {
ROS_ERROR("Could not find joint: %s", link->parent_joint->name.c_str());
ROS_ASSERT(false);
}
// add the joint to the chain
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
ROS_DEBUG("Joint axis: %f, %f, %f", joint->axis.x, joint->axis.y, joint->axis.z);
addJointToChainInfo(link->parent_joint, _solver_info);
}
link = robot_model.getLink(link->getParent()->name);
}
_solver_info.link_names.push_back(tip_name);
// We expect order from root to tip, so reverse the order
std::reverse(_solver_info.limits.begin(), _solver_info.limits.end());
std::reverse(_solver_info.joint_names.begin(), _solver_info.joint_names.end());
std::reverse(_solver_info.link_names.begin(), _solver_info.link_names.end());
}
/// /////////////////////////////////////////////////////////////////////////////
/// @brief load URDF model description from string and create search operations data structures
void URDFRenderer::loadURDFModel
(urdf::Model &model)
{
typedef std::vector<boost::shared_ptr<urdf::Link> > V_Link;
V_Link links;
model.getLinks(links);
V_Link::iterator it = links.begin();
V_Link::iterator end = links.end();
for (; it != end; ++it)
process_link (*it);
}
bool Kinematics::readJoints(urdf::Model &robot_model) {
num_joints = 0;
// get joint maxs and mins
boost::shared_ptr<const urdf::Link> link = robot_model.getLink(tip_name);
boost::shared_ptr<const urdf::Joint> joint;
while (link && link->name != root_name) {
joint = robot_model.getJoint(link->parent_joint->name);
if (!joint) {
ROS_ERROR("Could not find joint: %s",link->parent_joint->name.c_str());
return false;
}
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
ROS_INFO( "adding joint: [%s]", joint->name.c_str() );
num_joints++;
}
link = robot_model.getLink(link->getParent()->name);
}
joint_min.resize(num_joints);
joint_max.resize(num_joints);
info.joint_names.resize(num_joints);
info.link_names.resize(num_joints);
info.limits.resize(num_joints);
link = robot_model.getLink(tip_name);
unsigned int i = 0;
while (link && i < num_joints) {
joint = robot_model.getJoint(link->parent_joint->name);
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED) {
ROS_INFO( "getting bounds for joint: [%s]", joint->name.c_str() );
float lower, upper;
int hasLimits;
if ( joint->type != urdf::Joint::CONTINUOUS ) {
lower = joint->limits->lower;
upper = joint->limits->upper;
hasLimits = 1;
} else {
lower = -M_PI;
upper = M_PI;
hasLimits = 0;
}
int index = num_joints - i -1;
joint_min.data[index] = lower;
joint_max.data[index] = upper;
info.joint_names[index] = joint->name;
info.link_names[index] = link->name;
info.limits[index].joint_name = joint->name;
info.limits[index].has_position_limits = hasLimits;
info.limits[index].min_position = lower;
info.limits[index].max_position = upper;
i++;
}
link = robot_model.getLink(link->getParent()->name);
}
return true;
}
bool URDF_RBDL_Model::initFrom(const urdf::Model& model, const std::string& root)
{
// Display debug information
#if DISPLAY_DEBUG_INFO
ROS_ERROR("Initialising RBDL model of '%s' with root link '%s'...", model.getName().c_str(), root.c_str());
#endif
// Save the root link name
m_nameURDFRoot = model.getRoot()->name;
m_indexURDFRoot = (unsigned int) -1;
// Reset root mass to zero and give it the proper name
mBodies[0].mMass = 0;
// Set gravity to ROS standards
gravity << 0, 0, -9.81;
std::vector<boost::shared_ptr<urdf::Link> > links;
model.getLinks(links);
boost::shared_ptr<const urdf::Link> oldRoot = model.getRoot();
boost::shared_ptr<const urdf::Link> newRoot = model.getLink(root);
if(oldRoot == newRoot || !newRoot)
process(*oldRoot, 0, Math::SpatialTransform());
else
processReverse(*newRoot, 0, NULL, Math::SpatialTransform());
// Rename the root body to our URDF root
mBodyNameMap.erase("ROOT");
mBodyNameMap[root] = 0;
// Display debug information
#if DISPLAY_DEBUG_INFO
ROS_INFO("URDF root link is '%s' and was detected at RBDL body index %u", m_nameURDFRoot.c_str(), m_indexURDFRoot);
#endif
return true;
}
bool kdl_urdf_tools::initialize_kinematics_from_urdf(
const std::string &robot_description,
const std::string &root_link,
const std::string &tip_link,
unsigned int &n_dof,
KDL::Chain &kdl_chain,
KDL::Tree &kdl_tree,
urdf::Model &urdf_model)
{
if(robot_description.length() == 0) {
ROS_ERROR("URDF string is empty.");
return false;
}
// Construct an URDF model from the xml string
urdf_model.initString(robot_description);
// Get a KDL tree from the robot URDF
if (!kdl_parser::treeFromUrdfModel(urdf_model, kdl_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
// Populate the KDL chain
if(!kdl_tree.getChain(root_link, tip_link, kdl_chain))
{
ROS_ERROR_STREAM("Failed to get KDL chain from tree: ");
ROS_ERROR_STREAM(" "<<root_link<<" --> "<<tip_link);
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfJoints()<<" joints");
ROS_ERROR_STREAM(" Tree has "<<kdl_tree.getNrOfSegments()<<" segments");
ROS_ERROR_STREAM(" The segments are:");
KDL::SegmentMap segment_map = kdl_tree.getSegments();
KDL::SegmentMap::iterator it;
for( it=segment_map.begin();
it != segment_map.end();
it++ )
{
ROS_ERROR_STREAM( " "<<(*it).first);
}
return false;
}
// Store the number of degrees of freedom of the chain
n_dof = kdl_chain.getNrOfJoints();
return true;
}
bool get_model(urdf::Model& robot)
{
ros::NodeHandle nh;
std::string file;
nh.getParam("urdf_file_path", file);
TiXmlDocument robot_model_xml;
robot_model_xml.LoadFile(file);
TiXmlElement *robot_xml = robot_model_xml.FirstChildElement("robot");
if (!robot_xml){
std::cerr << "ERROR: Could not load the xml into TiXmlElement" << std::endl;
return false;
}
if (!robot.initXml(robot_xml)){
std::cerr << "ERROR: Model Parsing the xml failed" << std::endl;
return false;
}
return true;
}
/**
* TaskSpacePosition annotations.
*
* @return UIMA error id. UIMA_ERR_NONE on success.
*/
uima::TyErrorId annotateTaskSpace(
uima::CAS& cas,
const urdf::Model& model
) {
uima::FSIndexRepository& index = cas.getIndexRepository();
uima::FeatureStructure ts;
boost::shared_ptr<urdf::Link> link;
std::vector< boost::shared_ptr<urdf::Link> > links;
model.getLinks(links);
for (std::size_t i = 0, size = links.size(); i < size; i++) {
link = links[i];
ts = cas.createFS(TaskSpacePosition);
ts.setFSValue(tsXyzFtr,
utils::toDoubleArrayFS(cas, MongoUrdf::getPosition(link)));
ts.setFSValue(tsRpyFtr,
utils::toDoubleArrayFS(cas, MongoUrdf::getRotation(link)));
index.addFS(ts);
}
return UIMA_ERR_NONE;
}
bool Pod::init(hardware_interface::EffortJointInterface* hw, urdf::Model urdf) {
std::string joint_name;
nh_.param("command_timeout", command_timeout_, command_timeout_);
if (!nh_.getParam("joint", joint_name)) {
ROS_ERROR("No joint given (namespace: %s)", nh_.getNamespace().c_str());
return false;
}
joint_ = hw->getHandle(joint_name);
boost::shared_ptr<const urdf::Joint> joint_urdf = urdf.getJoint(joint_name);
if (!joint_urdf) {
ROS_ERROR("Could not find joint '%s' in urdf", joint_name.c_str());
return false;
}
if (!pid_controller_.init(ros::NodeHandle(nh_, "pid")))
return false;
controller_state_publisher_.reset(new realtime_tools::RealtimePublisher<control_msgs::JointControllerState>(nh_, "state", 1));
command_sub_ = nh_.subscribe<walrus_pod_controller::PodCommand>("command", 1, &Pod::setCommandCallback, this);
}
bool TreeKinematics::readJoints(urdf::Model &robot_model,
KDL::Tree &kdl_tree,
std::string &tree_root_name,
unsigned int &nr_of_jnts,
KDL::JntArray &joint_min,
KDL::JntArray &joint_max,
KDL::JntArray &joint_vel_max)
{
KDL::SegmentMap segmentMap;
segmentMap = kdl_tree.getSegments();
tree_root_name = kdl_tree.getRootSegment()->second.segment.getName();
nr_of_jnts = kdl_tree.getNrOfJoints();
ROS_DEBUG( "the tree's number of joints: [%d]", nr_of_jnts );
joint_min.resize(nr_of_jnts);
joint_max.resize(nr_of_jnts);
joint_vel_max.resize(nr_of_jnts);
info_.joint_names.resize(nr_of_jnts);
info_.limits.resize(nr_of_jnts);
// The following walks through all tree segments, extracts their joints except joints of KDL::None and extracts
// the information about min/max position and max velocity of the joints not of type urdf::Joint::UNKNOWN or
// urdf::Joint::FIXED
ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None.");
boost::shared_ptr<const urdf::Joint> joint;
for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); ++seg_it)
{
if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None)
{
// check, if joint can be found in the URDF model of the robot
joint = robot_model.getJoint(seg_it->second.segment.getJoint().getName().c_str());
if (!joint)
{
ROS_FATAL("Joint '%s' has not been found in the URDF robot model!", joint->name.c_str());
return false;
}
// extract joint information
if (joint->type != urdf::Joint::UNKNOWN && joint->type != urdf::Joint::FIXED)
{
ROS_DEBUG( "getting information about joint: [%s]", joint->name.c_str() );
double lower = 0.0, upper = 0.0, vel_limit = 0.0;
unsigned int has_pos_limits = 0, has_vel_limits = 0;
if ( joint->type != urdf::Joint::CONTINUOUS )
{
ROS_DEBUG("joint is not continuous.");
lower = joint->limits->lower;
upper = joint->limits->upper;
has_pos_limits = 1;
if (joint->limits->velocity)
{
has_vel_limits = 1;
vel_limit = joint->limits->velocity;
ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
}
else
{
has_vel_limits = 0;
vel_limit = 0.0;
ROS_DEBUG("joint has no velocity limit.");
}
}
else
{
ROS_DEBUG("joint is continuous.");
lower = -M_PI;
upper = M_PI;
has_pos_limits = 0;
if(joint->limits && joint->limits->velocity)
{
has_vel_limits = 1;
vel_limit = joint->limits->velocity;
ROS_DEBUG("joint has following velocity limit: %f", vel_limit);
}
else
{
has_vel_limits = 0;
vel_limit = 0.0;
ROS_DEBUG("joint has no velocity limit.");
}
}
joint_min(seg_it->second.q_nr) = lower;
joint_max(seg_it->second.q_nr) = upper;
joint_vel_max(seg_it->second.q_nr) = vel_limit;
ROS_DEBUG("pos_min = %f, pos_max = %f, vel_max = %f", lower, upper, vel_limit);
info_.joint_names[seg_it->second.q_nr] = joint->name;
info_.limits[seg_it->second.q_nr].joint_name = joint->name;
info_.limits[seg_it->second.q_nr].has_position_limits = has_pos_limits;
info_.limits[seg_it->second.q_nr].min_position = lower;
info_.limits[seg_it->second.q_nr].max_position = upper;
info_.limits[seg_it->second.q_nr].has_velocity_limits = has_vel_limits;
info_.limits[seg_it->second.q_nr].max_velocity = vel_limit;
}
}
}
return true;
}
// Create an object of class Joint that corresponds to the URDF joint specified
// by joint_name.
shared_ptr<JointBase> getJoint( const string& joint_name, const bool is_steer_joint,
const NodeHandle& ctrlr_nh,
const urdf::Model& urdf_model,
EffortJointInterface *const eff_joint_iface,
PositionJointInterface *const pos_joint_iface,
VelocityJointInterface *const vel_joint_iface)
{
if (eff_joint_iface != NULL)
{
JointHandle handle;
bool handle_found;
try
{
handle = eff_joint_iface->getHandle(joint_name);
handle_found = true;
}
catch (...)
{
handle_found = false;
}
if (handle_found)
{
const shared_ptr<const urdf::Joint> urdf_joint =
urdf_model.getJoint(joint_name);
if (urdf_joint == NULL)
{
throw runtime_error("\"" + joint_name +
"\" was not found in the URDF data.");
}
shared_ptr<JointBase> joint(new PIDJoint(handle, urdf_joint, ctrlr_nh));
return joint;
}
}
if (pos_joint_iface != NULL)
{
JointHandle handle;
bool handle_found;
try
{
handle = pos_joint_iface->getHandle(joint_name);
handle_found = true;
}
catch (...)
{
handle_found = false;
}
if (handle_found)
{
const shared_ptr<const urdf::Joint> urdf_joint =
urdf_model.getJoint(joint_name);
if (urdf_joint == NULL)
{
throw runtime_error("\"" + joint_name +
"\" was not found in the URDF data.");
}
shared_ptr<JointBase> joint(new PosJoint(handle, urdf_joint));
return joint;
}
}
if (vel_joint_iface != NULL)
{
JointHandle handle;
bool handle_found;
try
{
handle = vel_joint_iface->getHandle(joint_name);
handle_found = true;
}
catch (...)
{
handle_found = false;
}
if (handle_found)
{
const shared_ptr<const urdf::Joint> urdf_joint =
urdf_model.getJoint(joint_name);
if (urdf_joint == NULL)
{
throw runtime_error("\"" + joint_name +
"\" was not found in the URDF data.");
}
if (is_steer_joint)
{
shared_ptr<JointBase> joint(new PIDJoint(handle, urdf_joint, ctrlr_nh));
return joint;
}
shared_ptr<JointBase> joint(new VelJoint(handle, urdf_joint));
return joint;
}
}
throw runtime_error("No handle for \"" + joint_name + "\" was found.");
}
/**
* DistanceToLimit annotations.
*
* @return UIMA error id. UIMA_ERR_NONE on success.
*/
uima::TyErrorId annotateLimits(
uima::CAS& cas,
const urdf::Model& model
) {
uima::FSIndexRepository& index = cas.getIndexRepository();
uima::ANIndex jsIndex = cas.getAnnotationIndex(JointState);
uima::ANIterator jsIter = jsIndex.iterator();
uima::AnnotationFS js, dst;
uima::FeatureStructure jtp;
uima::StringArrayFS jsNames;
uima::DoubleArrayFS jtpPositions, jtpVelocities, jtpEfforts;
boost::shared_ptr<const urdf::Joint> joint;
boost::shared_ptr<urdf::JointLimits> limits;
std::vector<std::string> dstNames;
std::vector<double> upperLimits, lowerLimits, velocities, efforts;
while (jsIter.isValid()) {
js = jsIter.get();
jtp = js.getFSValue(jsJtpFtr);
// Get feature structure value arrays.
jsNames = js.getStringArrayFSValue(jsNameFtr);
jtpPositions = jtp.getDoubleArrayFSValue(jtpPosFtr);
jtpVelocities = jtp.getDoubleArrayFSValue(jtpEffFtr);
jtpEfforts = jtp.getDoubleArrayFSValue(jtpVelFtr);
// Clear storage vectors.
dstNames.clear();
upperLimits.clear();
lowerLimits.clear();
velocities.clear();
efforts.clear();
for (std::size_t i = 0; i < jsNames.size(); i++) {
joint = model.getJoint(jsNames.get(i).asUTF8());
limits = joint->limits;
// Limits are only relevant for some joint types.
if (
joint->type == urdf::Joint::REVOLUTE ||
joint->type == urdf::Joint::PRISMATIC
) {
dstNames.push_back(joint->name);
upperLimits.push_back(limits->upper - jtpPositions.get(i));
lowerLimits.push_back(limits->lower - jtpPositions.get(i));
velocities.push_back(limits->velocity - jtpVelocities.get(i));
efforts.push_back(limits->effort - jtpEfforts.get(i));
}
}
dst = cas.createAnnotation(DistanceToLimit,
js.getBeginPosition(), js.getEndPosition());
dst.setFSValue(dstNameFtr, utils::toStringArrayFS(cas, dstNames));
dst.setFSValue(dstUppFtr, utils::toDoubleArrayFS(cas, upperLimits));
dst.setFSValue(dstLowFtr, utils::toDoubleArrayFS(cas, lowerLimits));
dst.setFSValue(dstVelFtr, utils::toDoubleArrayFS(cas, velocities));
dst.setFSValue(dstEffFtr, utils::toDoubleArrayFS(cas, efforts));
index.addFS(dst);
jsIter.moveToNext();
}
return UIMA_ERR_NONE;
}
void processFeedback( const visualization_msgs::InteractiveMarkerFeedbackConstPtr &feedback )
{
std::ostringstream s;
s << "Feedback from marker '" << feedback->marker_name << "' "
<< " / control '" << feedback->control_name << "'";
switch ( feedback->event_type )
{
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_DOWN:
mouseInUse = true;
break;
case visualization_msgs::InteractiveMarkerFeedback::MOUSE_UP:
{
// Compute FK for end effectors that have changed
// Call IK to get the joint states
moveit_msgs::GetPositionFKRequest req;
req.fk_link_names.push_back("RightArm");
req.robot_state.joint_state = planState;
req.header.stamp = ros::Time::now();
moveit_msgs::GetPositionFKResponse resp;
gFKinClient.call(req, resp);
std::cerr << "Response: " << resp.pose_stamped[0] << std::endl;
if (resp.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
{
joyInt.currentPose = resp.pose_stamped[0];
}
else
{
ROS_ERROR_STREAM("Failed to solve FK: " << resp.error_code.val);
}
gRPosePublisher.publish(joyInt.currentPose);
mouseInUse = false;
break;
}
case visualization_msgs::InteractiveMarkerFeedback::POSE_UPDATE:
{
Eigen::AngleAxisf aa;
aa = Eigen::Quaternionf(feedback->pose.orientation.w,
feedback->pose.orientation.x,
feedback->pose.orientation.y,
feedback->pose.orientation.z);
boost::shared_ptr<const urdf::Joint> targetJoint = huboModel.getJoint(feedback->marker_name);
Eigen::Vector3f axisVector = hubo_motion_ros::toEVector3(targetJoint->axis);
float angle;
// Get sign of angle
if (aa.axis().dot(axisVector) < 0)
{
angle = -aa.angle();
}
else
{
angle = aa.angle();
}
// Trim angle to joint limits
if (angle > targetJoint->limits->upper)
{
angle = targetJoint->limits->upper;
}
else if (angle < targetJoint->limits->lower)
{
angle = targetJoint->limits->lower;
}
// Locate the index of the solution joint in the plan state
for (int j = 0; j < planState.name.size(); j++)
{
if (feedback->marker_name == planState.name[j])
{
planState.position[j] = angle;
}
}
prevAA = aa;
// Time and Frame stamps
planState.header.frame_id = "/Body_TSY";
planState.header.stamp = ros::Time::now();
gStatePublisher.publish(planState);
break;
}
}
gIntServer->applyChanges();
}
int main(int argc, char** argv)
{
ROS_INFO("Started fullbody_teleop.");
ros::init(argc, argv, "fullbody_teleop");
ros::NodeHandle nh;
std::string robotDescription;
if (!nh.getParam("/robot_description", robotDescription))
{
ROS_FATAL("Parameter for robot description not provided");
}
huboModel.initString(robotDescription);
//ros::Timer frame_timer = n.createTimer(ros::Duration(0.01), frameCallback);
gIntServer.reset( new interactive_markers::InteractiveMarkerServer("joint_controls","",false) );
for (auto jointPair : huboModel.joints_)
{
boost::shared_ptr<urdf::Joint> joint = jointPair.second;
if (joint->name[1] == 'F') { continue; }
if (joint->name== "TSY") { continue; }
makeJointMarker(joint->name);
}
std::cerr << "\nURDF size: " << huboModel.joints_.size() << std::endl;
ros::Duration(0.1).sleep();
for (int i = 0; i < HUBO_JOINT_COUNT; i++)
{
if (DRCHUBO_URDF_JOINT_NAMES[i] != "")
{
planState.name.push_back(DRCHUBO_URDF_JOINT_NAMES[i]);
planState.position.push_back(0);
planState.velocity.push_back(0);
planState.effort.push_back(0);
}
}
for (int side = 0; side < 2; side++)
for (int i = 1; i <= 3; i++)
for (int j = 1; j <= 3; j++)
{
std::string sideStr = (side == 0) ? "R" : "L";
planState.name.push_back(sideStr + "F" + std::to_string(i) + std::to_string(j));
planState.position.push_back(0);
planState.velocity.push_back(0);
planState.effort.push_back(0);
}
makeSaveButton();
gIntServer->applyChanges();
gJoySubscriber = nh.subscribe("joy_in", 1, &joyCallback);
gIKinClient = nh.serviceClient<moveit_msgs::GetPositionIK>("/hubo/kinematics/ik_service");
gFKinClient = nh.serviceClient<moveit_msgs::GetPositionFK>("/hubo/kinematics/fk_service");
gStatePublisher = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
gRPosePublisher = nh.advertise<geometry_msgs::PoseStamped>("rh_pose", 1);
gTextPublisher = nh.advertise<std_msgs::String>("text_out", 1);
gTimer = nh.createTimer(ros::Duration(1), &timerCallback);
gTimer.start();
ros::spin();
gIntServer.reset();
return 0;
}
inline bool modelHasMovableJoint(const urdf::Model& model, const std::string& name)
{
boost::shared_ptr<const urdf::Joint> joint = model.getJoint(name);
return joint.get() && isMovingJoint(joint->type);
}
bool ChainParser::parseChain(XmlRpc::XmlRpcValue& chain_description, Tree tree, urdf::Model& robot_model,
std::map<std::string, unsigned int>& joint_name_to_index,
std::vector<std::string>& index_to_joint_name,
std::vector<double>& q_min, std::vector<double>& q_max) {
ros::NodeHandle n("~");
std::string ns = n.getNamespace();
std::cout << chain_description << std::endl;
if (chain_description.getType() != XmlRpc::XmlRpcValue::TypeStruct) {
ROS_ERROR("Chain description should be a struct containing 'root' and 'tip'. (namespace: %s)", n.getNamespace().c_str());
return false;
}
XmlRpc::XmlRpcValue& v_root_link = chain_description["root"];
if (v_root_link.getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_ERROR("Chain description for does not contain 'root'. (namespace: %s)", n.getNamespace().c_str());
return false;
}
std::string root_link_name = (std::string)v_root_link;
XmlRpc::XmlRpcValue& v_tip_link = chain_description["tip"];
if (v_tip_link.getType() != XmlRpc::XmlRpcValue::TypeString) {
ROS_ERROR("Chain description for does not contain 'tip'. (namespace: %s)", n.getNamespace().c_str());
return false;
}
std::string tip_link_name = (std::string)v_tip_link;
Chain* chain = new Chain();
ROS_INFO("Looking for chain from %s to %s", root_link_name.c_str(), tip_link_name.c_str());
if (!tree.kdl_tree_.getChain(root_link_name, tip_link_name, chain->kdl_chain_)) {
// if (!tree.kdl_tree_.getChain("base", tip_link_name, chain->kdl_chain_)) {
ROS_FATAL("Could not initialize chain object");
return false;
}
for(unsigned int i = 0; i < chain->kdl_chain_.getNrOfSegments(); ++i) {
const KDL::Segment& segment = chain->kdl_chain_.getSegment(i);
const KDL::Joint& joint = segment.getJoint();
if (joint.getType() != KDL::Joint::None)
{
//cout << "Segment: " << segment.getName() << endl;
//cout << "Joint: " << joint.getName() << endl;
unsigned int full_joint_index = 0;
std::map<std::string, unsigned int>::iterator it_joint = joint_name_to_index.find(joint.getName());
if (it_joint == joint_name_to_index.end()) {
// joint name is not yet in map, so give it a new fresh index and add it to the map
full_joint_index = joint_name_to_index.size();
//cout << " new joint, gets index " << full_joint_index << endl;
//cout << " type: " << joint.getTypeName() << endl;
joint_name_to_index[joint.getName()] = full_joint_index;
index_to_joint_name.push_back(joint.getName());
//cout << " Lower limit: " << robot_model.getJoint(joint.getName())->limits->lower << endl;
//cout << " Upper limit: " << robot_model.getJoint(joint.getName())->limits->upper << endl;
// determine joint limits from URDF
q_min.push_back(robot_model.getJoint(joint.getName())->limits->lower);
q_max.push_back(robot_model.getJoint(joint.getName())->limits->upper);
} else {
// joint name already in the map, so look-up its index
full_joint_index = it_joint->second;
//cout << " existing joint, has index: " << full_joint_index << endl;
}
}
}
return true;
}