/** HACK! */ void Character::updateJointOrdering(){ if( jointOrder.size() == joints.size() ) return; // HACK assume ordering is ok jointOrder.clear(); if (!root) return; DynamicArray<ArticulatedRigidBody*> bodies; bodies.push_back(root); int currentBody = 0; while ((uint)currentBody<bodies.size()){ //add all the children joints to the list for (int i=0;i<bodies[currentBody]->getChildJointCount();i++){ Joint *j = bodies[currentBody]->getChildJoint(i); jointOrder.push_back( getJointIndex(j->getName()) ); bodies.push_back(bodies[currentBody]->getChildJoint(i)->getChild()); } currentBody++; } reverseJointOrder.resize( jointOrder.size() ); for( uint i=0; i < jointOrder.size(); ++i ) reverseJointOrder[jointOrder[i]] = i; }
/* Method to calculate the FK for the required joint configuration * @returns true if successful */ bool arm_kinematics::Kinematics::getPositionFK( std::string frame_id, const sensor_msgs::JointState &joint_configuration, geometry_msgs::PoseStamped &result) { KDL::Frame p_out; KDL::JntArray jnt_pos_in; tf::Stamped<tf::Pose> tf_pose; // Copying the positions of the joints relative to its index in the KDL chain jnt_pos_in.resize(num_joints); for (unsigned int i = 0; i < num_joints; i++) { int tmp_index = getJointIndex(joint_configuration.name[i]); if (tmp_index >= 0) jnt_pos_in(tmp_index) = joint_configuration.position[i]; } int num_segments = chain.getNrOfSegments(); ROS_DEBUG("Number of Segments in the KDL chain: %d", num_segments); if (fk_solver->JntToCart(jnt_pos_in, p_out, num_segments) >= 0) { tf_pose.frame_id_ = root_name; tf_pose.stamp_ = ros::Time(); tf::poseKDLToTF(p_out, tf_pose); try { tf_listener.transformPose(frame_id, tf_pose, tf_pose); } catch (...) { ROS_ERROR("Could not transform FK pose to frame: %s", frame_id.c_str()); return false; } tf::poseStampedTFToMsg(tf_pose, result); } else { ROS_ERROR("Could not compute FK for endpoint."); return false; } return true; }
bool extractJointState(const sensor_msgs::JointState& joint_state, const kinematics_msgs::KinematicSolverInfo &chain_info, std::vector<double>& values) { values.resize(chain_info.joint_names.size()); for(unsigned int i = 0; i < chain_info.joint_names.size(); i++) { const std::string& joint_name = chain_info.joint_names[i]; int values_ind = getJointIndex(joint_name, chain_info); if(values_ind == -1) { ROS_WARN_STREAM("Can't find joint " << joint_name << " in chain"); return false; } bool found = false; for(unsigned int j = 0; j < joint_state.name.size(); j++) { if(joint_state.name[j] == joint_name) { found = true; values[values_ind] = joint_state.position[j]; ROS_INFO_STREAM("Setting joint name " << joint_name << " ind " << values_ind << " value " << joint_state.position[j]); break; } } if(!found) { ROS_WARN_STREAM("Can't find joint " << joint_name << " in joint state"); return false; } } return true; }
IndexArray HuboDescription::getJointIndices(StringArray joint_names) const { IndexArray result; for(size_t i=0; i < joint_names.size(); ++i) { result.push_back(getJointIndex(joint_names[i])); } return result; }
bool PR2ArmKinematics::getPositionFK(kinematics_msgs::GetPositionFK::Request &request, kinematics_msgs::GetPositionFK::Response &response) { if(!active_) { ROS_ERROR("FK service not active"); return true; } if(!checkFKService(request,response,fk_solver_info_)) return true; KDL::Frame p_out; KDL::JntArray jnt_pos_in; geometry_msgs::PoseStamped pose; tf::Stamped<tf::Pose> tf_pose; jnt_pos_in.resize(dimension_); for(int i=0; i < (int) request.robot_state.joint_state.position.size(); i++) { int tmp_index = getJointIndex(request.robot_state.joint_state.name[i],fk_solver_info_); if(tmp_index >=0) jnt_pos_in(tmp_index) = request.robot_state.joint_state.position[i]; } response.pose_stamped.resize(request.fk_link_names.size()); response.fk_link_names.resize(request.fk_link_names.size()); bool valid = true; for(unsigned int i=0; i < request.fk_link_names.size(); i++) { ROS_DEBUG("End effector index: %d",pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i])); ROS_DEBUG("Chain indices: %d",kdl_chain_.getNrOfSegments()); if(jnt_to_pose_solver_->JntToCart(jnt_pos_in,p_out,pr2_arm_kinematics::getKDLSegmentIndex(kdl_chain_,request.fk_link_names[i])) >=0) { tf_pose.frame_id_ = root_name_; tf_pose.stamp_ = ros::Time(); tf::PoseKDLToTF(p_out,tf_pose); tf::poseStampedTFToMsg(tf_pose,pose); if(!transformPose(request.header.frame_id, pose, response.pose_stamped[i])) { response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; return false; } response.fk_link_names[i] = request.fk_link_names[i]; response.error_code.val = response.error_code.SUCCESS; } else { ROS_ERROR("Could not compute FK for %s",request.fk_link_names[i].c_str()); response.error_code.val = response.error_code.NO_FK_SOLUTION; valid = false; } } return true; }
//this assumes that everything has been checked and is in the correct frame bool PR2ArmKinematics::getPositionIKHelper(kinematics_msgs::GetPositionIK::Request &request, kinematics_msgs::GetPositionIK::Response &response) { KDL::Frame pose_desired; tf::poseMsgToKDL(request.ik_request.pose_stamped.pose, pose_desired); //Do the IK KDL::JntArray jnt_pos_in; KDL::JntArray jnt_pos_out; jnt_pos_in.resize(dimension_); for(int i=0; i < dimension_; i++) { int tmp_index = getJointIndex(request.ik_request.ik_seed_state.joint_state.name[i],ik_solver_info_); if(tmp_index >=0) { jnt_pos_in(tmp_index) = request.ik_request.ik_seed_state.joint_state.position[i]; } else { ROS_ERROR("i: %d, No joint index for %s",i,request.ik_request.ik_seed_state.joint_state.name[i].c_str()); } } int ik_valid = pr2_arm_ik_solver_->CartToJntSearch(jnt_pos_in, pose_desired, jnt_pos_out, request.timeout.toSec()); if(ik_valid == pr2_arm_kinematics::TIMED_OUT) response.error_code.val = response.error_code.TIMED_OUT; if(ik_valid == pr2_arm_kinematics::NO_IK_SOLUTION) response.error_code.val = response.error_code.NO_IK_SOLUTION; response.solution.joint_state.header = request.ik_request.pose_stamped.header; if(ik_valid >= 0) { response.solution.joint_state.name = ik_solver_info_.joint_names; response.solution.joint_state.position.resize(dimension_); for(int i=0; i < dimension_; i++) { response.solution.joint_state.position[i] = jnt_pos_out(i); ROS_DEBUG("IK Solution: %s %d: %f",response.solution.joint_state.name[i].c_str(),i,jnt_pos_out(i)); } response.error_code.val = response.error_code.SUCCESS; return true; } else { ROS_DEBUG("An IK solution could not be found"); return true; } }
/* Method to calculate the IK for the required end pose * @returns true if successful */ bool arm_kinematics::Kinematics::getPositionIK( const geometry_msgs::PoseStamped &pose_stamp, const sensor_msgs::JointState &seed, sensor_msgs::JointState *result) { geometry_msgs::PoseStamped pose_msg_in = pose_stamp; tf::Stamped<tf::Pose> transform; tf::Stamped<tf::Pose> transform_root; tf::poseStampedMsgToTF(pose_msg_in, transform); //Do the IK KDL::JntArray jnt_pos_in; KDL::JntArray jnt_pos_out; jnt_pos_in.resize(num_joints); // Copying the positions of the joints relative to its index in the KDL chain for (unsigned int i = 0; i < num_joints; i++) { int tmp_index = getJointIndex(seed.name[i]); if (tmp_index >= 0) { jnt_pos_in(tmp_index) = seed.position[i]; } else { ROS_ERROR("i: %d, No joint index for %s", i, seed.name[i].c_str()); } } //Convert F to our root_frame try { tf_listener.transformPose(root_name, transform, transform_root); } catch (...) { ROS_ERROR("Could not transform IK pose to frame: %s", root_name.c_str()); return false; } KDL::Frame F_dest; tf::transformTFToKDL(transform_root, F_dest); int ik_valid = ik_solver_pos->CartToJnt(jnt_pos_in, F_dest, jnt_pos_out); if (ik_valid >= 0) { result->name = info.joint_names; result->position.resize(num_joints); for (unsigned int i = 0; i < num_joints; i++) { result->position[i] = jnt_pos_out(i); ROS_DEBUG("IK Solution: %s %d: %f", result->name[i].c_str(), i, jnt_pos_out(i)); } return true; } else { ROS_DEBUG("An IK solution could not be found"); return false; } }
void reorderJointState(sensor_msgs::JointState &joint_state, const kinematics_msgs::KinematicSolverInfo &chain_info) { sensor_msgs::JointState tmp_state = joint_state; for(unsigned int i=0; i < joint_state.position.size(); i++) { int tmp_index = getJointIndex(joint_state.name[i],chain_info); if(tmp_index >=0) { tmp_state.position[tmp_index] = joint_state.position[i]; tmp_state.name[tmp_index] = joint_state.name[i]; } } joint_state = tmp_state; }
Joint* Robot::getJoint(const std::string &name) { int i = getJointIndex(name); return i >= 0 ? joints_[i] : NULL; }
bool Model::isJointNameUsed(const std::string jointName) { return (JOINT_INVALID_INDEX != getJointIndex(jointName)); }
bool TreeKinematics::getPositionIk(tree_kinematics::get_tree_position_ik::Request &request, tree_kinematics::get_tree_position_ik::Response &response) { ik_srv_duration_ = ros::Time::now().toSec(); ROS_DEBUG("getPositionIK method invoked."); // extract current joint positions from the request KDL::JntArray q, q_desi; double nr_of_jnts = request.pos_ik_request[0].ik_seed_state.joint_state.name.size(); q.resize(nr_of_jnts); q_desi.resize(nr_of_jnts); for (unsigned int i=0; i < nr_of_jnts; ++i) { int tmp_index = getJointIndex(request.pos_ik_request[0].ik_seed_state.joint_state.name[i]); if (tmp_index >=0) { q(tmp_index) = request.pos_ik_request[0].ik_seed_state.joint_state.position[i]; ROS_DEBUG("joint '%s' is now number '%d'", request.pos_ik_request[0].ik_seed_state.joint_state.name[i].c_str(), tmp_index); } else { ROS_FATAL("i: %d, No joint index for %s!",i,request.pos_ik_request[0].ik_seed_state.joint_state.name[i].c_str()); ROS_FATAL("Service call aborted."); return false; } } // convert pose messages into transforms from the root frame to the given poses and further into KDL::Frames geometry_msgs::PoseStamped pose_msg_in; geometry_msgs::PoseStamped pose_msg_transformed; tf::Stamped<tf::Pose> transform; tf::Stamped<tf::Pose> transform_root; KDL::Frames desired_poses; KDL::Frame desired_pose; for(unsigned int i = 0; i < request.pos_ik_request.size(); ++i) { pose_msg_in = request.pos_ik_request[i].pose_stamped; try { tf_listener_.waitForTransform(tree_root_name_, pose_msg_in.header.frame_id, pose_msg_in.header.stamp, ros::Duration(0.1)); tf_listener_.transformPose(tree_root_name_, pose_msg_in, pose_msg_transformed); } catch (tf::TransformException const &ex) { ROS_FATAL("%s",ex.what()); ROS_FATAL("Could not transform IK pose from '%s' to frame '%s'", tree_root_name_.c_str(), pose_msg_in.header.frame_id.c_str()); response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; return false; } tf::PoseMsgToKDL(pose_msg_transformed.pose, desired_pose); desired_poses[request.pos_ik_request[i].ik_link_name] = desired_pose; } // use the solver to compute desired joint positions ik_duration_ = ros::Time::now().toSec(); int ik_ret = ik_pos_solver_->CartToJnt_it(q, desired_poses, q_desi); // NOTE: Before it was CartToJnt (without the _it). What's the difference? ik_duration_ = ros::Time::now().toSec() - ik_duration_; ik_duration_median_ = ((ik_duration_median_ * (loop_count_ - 1)) + ik_duration_) / loop_count_; // insert the solver's result into the service response if (ik_ret >= 0 || ik_ret == -3) { response.solution.joint_state.name = info_.joint_names; response.solution.joint_state.position.resize(nr_of_jnts_); response.solution.joint_state.velocity.resize(nr_of_jnts_); for (unsigned int i=0; i < nr_of_jnts_; ++i) { response.solution.joint_state.position[i] = q_desi(i); response.solution.joint_state.velocity[i] = (q_desi(i) - q(i)) * srv_call_frequency_; ROS_DEBUG("IK Solution: %s %d: pos = %f, vel = %f",response.solution.joint_state.name[i].c_str(), i, response.solution.joint_state.position[i], response.solution.joint_state.velocity[i]); } response.error_code.val = response.error_code.SUCCESS; ik_srv_duration_ = ros::Time::now().toSec() - ik_srv_duration_; ik_srv_duration_median_ = ((ik_srv_duration_median_ * (loop_count_ - 1)) + ik_srv_duration_) / loop_count_; loop_count_++; ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_srv_duration %f and median %f", ik_srv_duration_, ik_srv_duration_median_); ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_duration %f and median %f", ik_duration_, ik_duration_median_); if (ik_ret == -3) { ROS_WARN_THROTTLE(1.0, "Maximum iterations reached! (error code = %d)", ik_ret); } } else { ROS_WARN("An IK solution could not be found (error code = %d)", ik_ret); response.error_code.val = response.error_code.NO_IK_SOLUTION; ik_srv_duration_ = ros::Time::now().toSec() - ik_srv_duration_; ik_srv_duration_median_ = ((ik_srv_duration_median_ * (loop_count_ - 1)) + ik_srv_duration_) / loop_count_; loop_count_++; ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_srv_duration %f and median %f", ik_srv_duration_, ik_srv_duration_median_); ROS_INFO_THROTTLE(1.0, "tree_kinematics: ik_duration %f and median %f", ik_duration_, ik_duration_median_); } return true; }
bool TreeKinematics::getPositionFk(kinematics_msgs::GetPositionFK::Request& request, kinematics_msgs::GetPositionFK::Response& response) { ROS_DEBUG("getPositionFK method invoked."); KDL::JntArray q_in; double nr_of_jnts = request.robot_state.joint_state.name.size(); q_in.resize(nr_of_jnts); for (unsigned int i=0; i < nr_of_jnts; ++i) { int tmp_index = getJointIndex(request.robot_state.joint_state.name[i]); if (tmp_index >=0) { q_in(tmp_index) = request.robot_state.joint_state.position[i]; ROS_DEBUG("joint '%s' is now number '%d'", request.robot_state.joint_state.name[i].c_str(), tmp_index); } else { ROS_FATAL("i: %d, No joint index for %s!", i, request.robot_state.joint_state.name[i].c_str()); ROS_FATAL("Service call aborted."); return false; } } response.pose_stamped.resize(request.fk_link_names.size()); response.fk_link_names.resize(request.fk_link_names.size()); KDL::Frame p_out; geometry_msgs::PoseStamped pose; tf::Stamped<tf::Pose> tf_pose; for (unsigned int i=0; i < request.fk_link_names.size(); i++) { ROS_DEBUG("End effector name: %s",request.fk_link_names[i].c_str()); int fk_ret = fk_solver_->JntToCart(q_in, p_out, request.fk_link_names[i]); if (fk_ret >= 0) { tf_pose.frame_id_ = tree_root_name_; tf_pose.stamp_ = ros::Time::now(); tf::PoseKDLToTF(p_out, tf_pose); try { tf_listener_.transformPose(request.header.frame_id, tf_pose, tf_pose); } catch (tf::TransformException const &ex) { ROS_FATAL("Could not transform FK pose to frame: %s",request.header.frame_id.c_str()); response.error_code.val = response.error_code.FRAME_TRANSFORM_FAILURE; return false; } tf::poseStampedTFToMsg(tf_pose, pose); response.pose_stamped[i] = pose; response.fk_link_names[i] = request.fk_link_names[i]; response.error_code.val = response.error_code.SUCCESS; } else { ROS_WARN("A FK solution could not be found for %s (error code = %d)", request.fk_link_names[i].c_str(), fk_ret); response.error_code.val = response.error_code.NO_FK_SOLUTION; } } return true; }