示例#1
0
void KdlTreeFk::recursiveGetVels(bool getAll, const KDL::FrameVel &baseFrame,
                                 const KDL::SegmentMap::const_iterator &it, const KDL::JntArrayVel &joints, std::map<std::string, KDL::FrameVel> &frames)
{
    /// get FrameVel for this segment and store as needed
    const std::string& segmentName  = it->first;
    const KDL::TreeElement& element = it->second;
    const KDL::Segment& segment     = element.segment;
    unsigned int q_nr               = element.q_nr;
    KDL::FrameVel currentFrame      = baseFrame * KDL::FrameVel(segment.pose(joints.q(q_nr)),
                                      segment.twist(joints.q(q_nr), joints.qdot(q_nr)));
    if (getAll)
    {
        frames[segmentName] = currentFrame;
    }
    else
    {
        std::map<std::string, KDL::FrameVel>::iterator frameIt = frames.find(segmentName);
        if (frameIt != frames.end())
        {
            frameIt->second = currentFrame;
        }
    }

    /// recurse
    const std::vector<KDL::SegmentMap::const_iterator>& children = element.children;
    for (std::vector<KDL::SegmentMap::const_iterator>::const_iterator childIt = children.begin();
            childIt != children.end(); ++childIt)
    {
        recursiveGetVels(getAll, currentFrame, *childIt, joints, frames);
    }
}
bool IKController::compute_ik(
    const KDL::JntArrayVel &positions,
    const KDL::Frame &tip_frame_des,
    KDL::JntArray &ik_hint,
    KDL::JntArrayVel &positions_des,
    KDL::JntArrayVel &positions_des_last,
    const bool debug, 
    const ros::Duration dt)
{
  // Select ik hint by joint
  for(unsigned int i=0; i<n_dof_; i++) {
    switch(hint_modes_[i]) {
      case 0:
        ik_hint(i) = positions_.q(i); break;
      case 1:
        ik_hint(i) = (joint_limits_min_(i) + joint_limits_max_(i))/2.0; break;
      case 2:
        ik_hint(i) = hint_positions_(i); break;
    };
  }

  // Compute joint coordinates of the target tip frame
  int ik_ret = kdl_ik_solver_pos_->CartToJnt(ik_hint, tip_frame_des, positions_des.q);

  if(ik_ret < 0) {
    return false;
  }

  if(debug) {
    RTT::log(RTT::Debug)<<"Unwrapped angles: "<<(positions_des.q.data.transpose)()<<RTT::endlog();
  }

  // Unwrap angles
  for(unsigned int i=0; i<n_dof_; i++) {
    if(positions_des.q(i) > 0) {
      positions_des.q(i) = fmod(positions_des.q(i)+M_PI,2.0*M_PI)-M_PI;
    } else {
      positions_des.q(i) = fmod(positions_des.q(i)-M_PI,2.0*M_PI)+M_PI;
    }
  }

  // Compute joint velocities
  //if(dt.toSec() > 1E-5) {
    //positions_des_.qdot.data = (positions_des_.q.data - positions_des_last_.q.data)/dt.toSec();
  //} else {
    positions_des.qdot.data.setZero();
  //}

  if(debug) {
    RTT::log(RTT::Debug)<<"Wrapped angles: "<<(positions_des.q.data.transpose())<<RTT::endlog();
    RTT::log(RTT::Debug)<<"Current position: "<<positions.q.data.transpose()<<RTT::endlog();
    RTT::log(RTT::Debug)<<"Current velocity: "<<positions.qdot.data.transpose()<<RTT::endlog();
    RTT::log(RTT::Debug)<<"IK result: "<<positions_des.q.data.transpose()<<RTT::endlog();
    RTT::log(RTT::Debug)<<"Displacement: "<<(positions_des.q.data-positions_.q.data).transpose()<<RTT::endlog();
  }

  return true;
}