/**
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;
}
