C++ kdl::JntArray类代码示例

int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init,
                              const KDL::Frame& p_in,
                              std::vector<KDL::JntArray> &q_out)
{
  Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
  std::vector<std::vector<double> > solution_ik;
  KDL::JntArray q;

  if(free_angle_ == 0)
  {
    pr2_arm_ik_.computeIKShoulderPan(b, q_init(0), solution_ik);
  }
  else
  {
    pr2_arm_ik_.computeIKShoulderRoll(b, q_init(2), solution_ik);
  }

  if(solution_ik.empty())
    return -1;

  q.resize(7);
  q_out.clear();
  for(int i=0; i< (int) solution_ik.size(); ++i)
  {
    for(int j=0; j < 7; j++)
    {
      q(j) = solution_ik[i][j];
    }
    q_out.push_back(q);
  }
  return 1;
}

//NEVER call this without setting the container chain!!
void kinematics_utilities::initialize_solvers(chain_and_solvers* container, KDL::JntArray& joints_value,KDL::JntArray& q_max, KDL::JntArray& q_min, int index)
{
    for (KDL::Segment& segment: container->chain.segments)
    {
        if (segment.getJoint().getType()==KDL::Joint::None) continue;
        //std::cout<<segment.getJoint().getName()<<std::endl;
        container->joint_names.push_back(segment.getJoint().getName());
    }
    assert(container->joint_names.size()==container->chain.getNrOfJoints());
    joints_value.resize(container->chain.getNrOfJoints());
    SetToZero(joints_value);
    q_max.resize(container->chain.getNrOfJoints());
    q_min.resize(container->chain.getNrOfJoints());
    container->average_joints.resize(container->chain.getNrOfJoints());
    container->fksolver=new KDL::ChainFkSolverPos_recursive(container->chain);
    container->ikvelsolver = new KDL::ChainIkSolverVel_pinv(container->chain);
    container->index=index;
    int j=0;
    for (auto joint_name:container->joint_names)
    {
#if IGNORE_JOINT_LIMITS
        q_max(j)=M_PI/3.0;
        q_min(j)=-M_PI/3.0;
#else
        q_max(j)=urdf_model.joints_[joint_name]->limits->upper;
        q_min(j)=urdf_model.joints_[joint_name]->limits->lower;
#endif
        container->average_joints(j)=(q_max(j)+q_min(j))/2.0;
        j++;
    }
    container->iksolver= new KDL::ChainIkSolverPos_NR_JL(container->chain,q_min,q_max,*container->fksolver,*container->ikvelsolver);
}

 Eigen::VectorXd toEigen(const KDL::Twist & v, const KDL::JntArray & dq)
 {
     VectorXd ret(6+dq.rows());
     ret.segment(0,6) = toEigen(v);
     for(int i=0; i < (int)dq.rows(); i++ ) { ret(6+i) = dq(i); }
     return ret;
 }

void KDLChainWrapper::jointArrayToRealJointArray(const KDL::JntArray& joint_array, KDL::JntArray& real_joint_array)
{
    ROS_ASSERT(int(joint_array.rows()) == num_joints_);
    ROS_ASSERT(int(real_joint_array.rows()) == num_real_joints_);
    for (int i=0; i<num_real_joints_; ++i)
    {
        real_joint_array(i) = mimic_joints_[i].offset + mimic_joints_[i].multiplier *
                              joint_array(mimic_joints_[i].mimic_joint);
    }
}

bool ArmAnalyticalInverseKinematics::isSolutionValid(const KDL::JntArray &solution) const
{
	bool valid = true;

	if (solution.rows() != 5) return false;

	for (unsigned int i = 0; i < solution.rows(); i++) {
		if ((solution(i) < _min_angles[i]) || (solution(i) > _max_angles[i])) {
			valid = false;
		}
	}

	return valid;
}

void Arm_Cartesian_Control::checkLimits(
		double dt, KDL::JntArray& joint_positions,
		KDL::JntArray& jntVel) {

	if (upper_joint_limits.size() < arm_chain->getNrOfJoints()
			|| lower_joint_limits.size() < arm_chain->getNrOfJoints()) {
	        std::cout << "No Joint limits defined";
		return;
	}

	double limit_range = 0.1;
        
        jointLimitsReached = false;

	for (int i=0; i<jntVel.data.rows(); i++) {
		double pos = joint_positions.data(i);
		double vel = jntVel.data(i);

		double fpos = pos + vel * dt * 2;

		double upper_limit = this->upper_joint_limits[i];
		double lower_limit = this->lower_joint_limits[i];

		double upper_clearance = upper_limit - fpos;
		double lower_clearance = lower_limit - fpos;

		double limit_vel = vel;


		if (fabs(upper_clearance) < limit_range*2  && vel > 0) {
			limit_vel *= upper_clearance;
		}
		if (fabs(lower_clearance) < limit_range*2  && vel < 0) {
			limit_vel *= lower_clearance;
		}

		if (fpos > upper_limit - limit_range && vel > 0) {
			limit_vel = 0.0;
		        std::cout << "Upper limit reached for joint " << i << std::endl;
                        jointLimitsReached = true;
 		} else if (fpos < lower_limit + limit_range && vel < 0) {
 			limit_vel = 0.0;
			std::cout << "Lower limit reached for joint " << i << std::endl;
                        jointLimitsReached = true;
		}

		jntVel.data(i) = limit_vel;

	}
}

void KdlTreeFk::getPoses(const KDL::JntArray& joints, std::map<std::string, KDL::Frame>& frames)
{
    if (joints.rows() != tree.getNrOfJoints())
    {
        std::stringstream err;
        err << "getPoses() joints not properly sized";
        //RCS::Logger::log("gov.nasa.controllers.KdlTreeFk", log4cpp::Priority::ERROR, err.str());
        throw std::runtime_error(err.str());
        return;
    }
    KDL::Frame baseFrame;
    if (frames.empty())
    {
        recursiveGetPoses(true, baseFrame, tree.getRootSegment(), joints, frames);
    }
    else
    {
        /// make sure all of the listed frames are available
        // if non existant frames are requested, this is the only way to find out
        for (std::map<std::string, KDL::Frame>::const_iterator it = frames.begin(); it != frames.end(); ++it)
        {
            if (tree.getSegment(it->first) == tree.getSegments().end())
            {
                std::stringstream err;
                err << "KdlTreeFk::getPoses() requested an unavailable pose";
                //RCS::Logger::log("gov.nasa.controllers.KdlTreeFk", log4cpp::Priority::ERROR, err.str());
                throw std::runtime_error(err.str());
                return;
            }
        }
        recursiveGetPoses(false, baseFrame, tree.getRootSegment(), joints, frames);
    }
}

bool FKSolver::solve(const KDL::JntArray& q_in, std::vector<KDL::Vector>& joint_pos, std::vector<KDL::Vector>& joint_axis, std::vector<KDL::Frame>& segment_frames) const
{
  Frame p_out = Frame::Identity();

  if (q_in.rows() != num_joints_)
    return false;

  joint_pos.resize(num_joints_);
  joint_axis.resize(num_joints_);
  segment_frames.resize(num_segments_);

  int j = 0;
  for (unsigned int i = 0; i < num_segments_; i++)
  {
    double joint_value = 0.0;
    if (chain_.getSegment(i).getJoint().getType() != Joint::None)
    {
      joint_value = q_in(j);
      joint_pos[j] = p_out * chain_.getSegment(i).getJoint().JointOrigin();
      joint_axis[j] = p_out.M * chain_.getSegment(i).getJoint().JointAxis();
      j++;
    }
    p_out = p_out * chain_.getSegment(i).pose(joint_value);
    segment_frames[i] = p_out;
  }
  return true;
}

int SNS_IK::CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in,
                      const KDL::JntArray& q_bias,
                      const std::vector<std::string>& biasNames,
                      KDL::JntArray &q_out, const KDL::Twist& bounds) {

  if (!m_initialized) {
    ROS_ERROR("SNS_IK was not properly initialized with a valid chain or limits.");
    return -1;
  }

  // The position solver uses a barrier function instead of the hard position limits
  m_ik_vel_solver->usePositionLimits(false);

  int result;
  if (q_bias.rows()) {
    MatrixD ns_jacobian;
    std::vector<int> indicies;
    if (!nullspaceBiasTask(q_bias, biasNames, &ns_jacobian, &indicies)) {
      ROS_ERROR("Could not create nullspace bias task");
      result = -1;
    } else {
      result = m_ik_pos_solver->CartToJnt(q_init, p_in, q_bias, ns_jacobian, indicies,
                                          m_nullspaceGain, &q_out, bounds);
    }
  } else {
    result = m_ik_pos_solver->CartToJnt(q_init, p_in, &q_out, bounds);
  }

  m_ik_vel_solver->usePositionLimits(true);
  return result;
}

void PostureControl::initialize(const KDL::JntArray& q_min, const KDL::JntArray& q_max, const std::vector<double>& q0, const std::vector<double>& gain, const std::map<std::string, unsigned int>& joint_name_to_index) {

    num_joints_ = q_min.rows();
    q0_.resize(num_joints_);
    q0_default_.resize(num_joints_);
    q_min_.resize(num_joints_);
    q_max_.resize(num_joints_);
    K_.resize(num_joints_);
    joint_name_to_index_ = joint_name_to_index;
    for (uint i = 0; i < num_joints_; i++) {
        q0_[i] = q0[i];
        q0_default_[i] = q0[i];
        q_min_[i] = q_min(i);
        q_max_[i] = q_max(i);
    }

    ROS_INFO("Length joint array = %i",num_joints_);
    for (uint i = 0; i < num_joints_; i++) {
        K_[i] = gain[i] / ((q_max(i) - q_min(i))*(q_max(i) - q_min(i)));
        //ROS_INFO("qmin = %f, qmax = %f, q0 = %f, K = %f, gain = %f",q_min(i),q_max(i),q0_[i], K_[i], gain[i]);
    }
    //ROS_INFO("Length joint array = %i",num_joints_);

    current_cost_ = 0;

}

double InverseKinematicsSolver::computeEuclideanDistance(
		const KDL::JntArray &array_1,
		const KDL::JntArray &array_2)
{
	double distance = 0.0;

	for (unsigned int i = 0; i < array_1.rows(); i++) {
		distance += (array_1(i) - array_2(i)) * (array_1(i) - array_2(i));
	}

	return sqrt(distance);
}

KDL::Frame ArmKinematics::get_pos_fk(const KDL::JntArray& q)
{
  assert(q.rows() == dof_);

  assert(fk_solver_);

  KDL::Frame pose;

  fk_solver_->JntToCart(q, pose);

  return pose;
}

int PR2ArmIKSolver::CartToJnt(const KDL::JntArray& q_init, 
                              const KDL::Frame& p_in, 
                              KDL::JntArray &q_out)
{
  Eigen::Matrix4f b = KDLToEigenMatrix(p_in);
  std::vector<std::vector<double> > solution_ik;
  if(free_angle_ == 0)
  {
    ROS_DEBUG("Solving with %f",q_init(0)); 
    pr2_arm_ik_.computeIKShoulderPan(b,q_init(0),solution_ik);
  }
  else
  {
    pr2_arm_ik_.computeIKShoulderRoll(b,q_init(2),solution_ik);
  }
  
  if(solution_ik.empty())
    return -1;

  double min_distance = 1e6;
  int min_index = -1;

  for(int i=0; i< (int) solution_ik.size(); i++)
  {     
    ROS_DEBUG("Solution : %d",(int)solution_ik.size());

    for(int j=0; j < (int)solution_ik[i].size(); j++)
    {   
      ROS_DEBUG("%d: %f",j,solution_ik[i][j]);
    }
    ROS_DEBUG(" ");
    ROS_DEBUG(" ");

    double tmp_distance = computeEuclideanDistance(solution_ik[i],q_init);
    if(tmp_distance < min_distance)
    {
      min_distance = tmp_distance;
      min_index = i;
    }
  }

  if(min_index > -1)
  {
    q_out.resize((int)solution_ik[min_index].size());
    for(int i=0; i < (int)solution_ik[min_index].size(); i++)
    {   
      q_out(i) = solution_ik[min_index][i];
    }
    return 1;
  }
  else
    return -1;
}

bool RobotStatus::reachedPosition(KDL::JntArray reference)
{
    if( reference.rows() != m_n_joints_monitoring )
    {
        ERROR_STREAM("reachedPosition check for " << reference.rows() << " joints while robot status contains " << m_n_joints_monitoring << " joints");
        return false;
    }

    if( m_pos_reached_threshold < 0.0)
    {
        ERROR_STREAM("Check for reachedPosition check while threshold was not set!");
        return false;
    }

    for( int i = 0; i < m_n_joints_monitoring; ++i)
    {
        if( fabs( reference(i) - m_joints_to_monitor(i)) > m_pos_reached_threshold )
            return false;
    }

    return true;
}

///reads Velocity of all manipulator joints
///@param data returns the Velocity per joint
void YouBotKDLInterface::getJointVelocity(KDL::JntArray& data)
{
    data.resize(ARMJOINTS);
	std::vector<youbot::JointSensedVelocity> jointSensedVelocity;
	jointSensedVelocity.resize(ARMJOINTS);  
	youBotManipulator->getJointData(jointSensedVelocity);
	
	data(0) = (double) jointSensedVelocity[0].angularVelocity.value();
	data(1) = (double) jointSensedVelocity[1].angularVelocity.value();
	data(2) = (double) jointSensedVelocity[2].angularVelocity.value();
	data(3) = (double) jointSensedVelocity[3].angularVelocity.value();
	data(4) = (double) jointSensedVelocity[4].angularVelocity.value();
	
}