C++ JntArray::resize方法代码示例

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

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

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

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

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

///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();
	
}

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

bool FkLookup::ChainFK::parseJointState(const sensor_msgs::JointState &state_in, KDL::JntArray &array_out) const{
    unsigned int num = state_in.name.size();
    int joints_needed = joints.size();
    array_out.resize(joints_needed);
    if(num == state_in.position.size()){
	for(unsigned i = 0; i <  num; ++i){
	    std::map<std::string, unsigned int>::const_iterator it = joints.find(state_in.name[i]);
	    if( it != joints.end()){
		array_out(it->second) = state_in.position[i];
		--joints_needed;
	    }
	}
    }
    return joints_needed == 0;
}

bool FkLookup::ChainFK::parseJointState(const sensor_msgs::JointState &state_in, KDL::JntArray &array_out, std::map<std::string, unsigned int> &missing) const{
    // seed state > robot_state > planning_scene
   if(missing.size() == 0){
        array_out.resize(joints.size());
        missing = joints;
    }
    int joints_needed = missing.size();
    unsigned int num = state_in.name.size();
    if(state_in.name.size() == state_in.position.size()){
	for(unsigned i = 0; i <  num && joints_needed > 0; ++i){
	    std::map<std::string, unsigned int>::iterator it = missing.find(state_in.name[i]);
	    if( it != missing.end()){
		array_out(it->second) = state_in.position[i];
		--joints_needed;
	    }
	}
    }
    return joints_needed == 0;
}

int InverseKinematicsSolver::CartToJnt(const KDL::JntArray& q_init,
		const KDL::Frame& p_in,
		KDL::JntArray &q_out)
{
	ROS_DEBUG("InverseKinematicsSolver::CartToJnt\n");


	std::vector<KDL::JntArray> solution_ik;
	_ik.CartToJnt(q_init, p_in, solution_ik);

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

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

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

		for (unsigned int j = 0; j < solution_ik[i].rows(); 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(solution_ik[min_index].rows());
		for (unsigned int i = 0; i < solution_ik[min_index].rows(); i++) {
			q_out(i) = solution_ik[min_index](i);
		}
		return 1;
	} else {
		return -1;
	}
}

    void ITRCartesianImpedanceController::multiplyJacobian(const KDL::Jacobian& jac, const KDL::Wrench& src, KDL::JntArray& dest)
    {
        Eigen::Matrix<double,6,1> w;
        w(0) = src.force(0);
        w(1) = src.force(1);
        w(2) = src.force(2);
        w(3) = src.torque(0);
        w(4) = src.torque(1);
        w(5) = src.torque(2);

        Eigen::MatrixXd j(jac.rows(), jac.columns());
        j = jac.data;
        j.transposeInPlace();

        Eigen::VectorXd t(jac.columns());
        t = j*w;

        dest.resize(jac.columns());
        for (unsigned i=0; i<jac.columns(); i++)
        dest(i) = t(i);
    }

    VirtualForcePublisher()
    {
        ros::NodeHandle n_tilde("~");
        ros::NodeHandle n;

        // subscribe to joint state
        joint_state_sub_ = n.subscribe("joint_states", 1, &VirtualForcePublisher::callbackJointState, this);
        wrench_stamped_pub_ = n.advertise<geometry_msgs::WrenchStamped>("wrench", 1);

        // set publish frequency
        double publish_freq;
        n_tilde.param("publish_frequency", publish_freq, 50.0);
        publish_interval_ = ros::Duration(1.0/std::max(publish_freq,1.0));

        //set time constant of low pass filter
        n_tilde.param("time_constant", t_const_, 0.3);
        // set root and tip
        n_tilde.param("root", root, std::string("torso_lift_link"));
        n_tilde.param("tip", tip, std::string("l_gripper_tool_frame"));

        // load robot model
        urdf::Model robot_model;
        robot_model.initParam("robot_description");

        KDL::Tree kdl_tree;
        kdl_parser::treeFromUrdfModel(robot_model, kdl_tree);
        kdl_tree.getChain(root, tip, chain_);
        jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(chain_));

        jnt_pos_.resize(chain_.getNrOfJoints());
        jacobian_.resize(chain_.getNrOfJoints());

        for (size_t i=0; i<chain_.getNrOfSegments(); i++) {
            if (chain_.getSegment(i).getJoint().getType() != KDL::Joint::None) {
                std::cerr << "kdl_chain(" << i << ") " << chain_.getSegment(i).getJoint().getName().c_str() << std::endl;
            }
        }
    }

    void callbackJointState(const JointStateConstPtr& state)
    {
        std::map<std::string, double> joint_name_position;
        if (state->name.size() != state->position.size()) {
            ROS_ERROR("Robot state publisher received an invalid joint state vector");
            return;
        }

        // determine least recently published joint
        ros::Time last_published = ros::Time::now();
        for (unsigned int i=0; i<state->name.size(); i++) {
            ros::Time t = last_publish_time_[state->name[i]];
            last_published = (t < last_published) ? t : last_published;
        }

        if (state->header.stamp >= last_published + publish_interval_) {
            // get joint positions from state message
            std::map<std::string, double> joint_positions;
            std::map<std::string, double> joint_efforts;
            for (unsigned int i=0; i<state->name.size(); i++) {
                joint_positions.insert(make_pair(state->name[i], state->position[i]));
                joint_efforts.insert(make_pair(state->name[i], state->effort[i]));
            }

            KDL::JntArray  tau;
            KDL::Wrench    F;
            Eigen::Matrix<double,Eigen::Dynamic,6> jac_t;
            Eigen::Matrix<double,6,Eigen::Dynamic> jac_t_pseudo_inv;
            tf::StampedTransform transform;
            KDL::Wrench    F_pub;
            tf::Vector3 tf_force;
            tf::Vector3 tf_torque;

            tau.resize(chain_.getNrOfJoints());

            //getPositions;
            for (size_t i=0, j=0; i<chain_.getNrOfSegments(); i++) {
                if (chain_.getSegment(i).getJoint().getType() == KDL::Joint::None)
                    continue;

                // check
                std::string joint_name = chain_.getSegment(i).getJoint().getName();
                if ( joint_positions.find(joint_name) == joint_positions.end() ) {
                    ROS_ERROR("Joint '%s' is not found in joint state vector", joint_name.c_str());
                }

                // set position
                jnt_pos_(j) = joint_positions[joint_name];
                tau(j) = joint_efforts[joint_name];
                j++;
            }

            jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
            jac_t = jacobian_.data.transpose();
            if ( jacobian_.columns() >= jacobian_.rows() ) {
                jac_t_pseudo_inv =(jac_t.transpose() * jac_t).inverse() *  jac_t.transpose();
            } else {
                jac_t_pseudo_inv =jac_t.transpose() * ( jac_t *  jac_t.transpose() ).inverse();
            }
#if 1
            {
                ROS_INFO("jac_t# jac_t : ");
                Eigen::Matrix<double,6,6> mat_i =  mat_i = jac_t_pseudo_inv * jac_t;
                for (unsigned int i = 0; i < 6; i++) {
                    std::stringstream ss;
                    for (unsigned int j=0; j<6; j++)
                        ss << std::fixed << std::setw(8) << std::setprecision(4) << mat_i(j,i) << " ";
                    ROS_INFO_STREAM(ss.str());
                }
            }
#endif
            // f = - inv(jt) * effort
            for (unsigned int j=0; j<6; j++)
            {
                F(j) = 0;
                for (unsigned int i = 0; i < jnt_pos_.rows(); i++)
                {
                    F(j) += -1 * jac_t_pseudo_inv(j, i) * tau(i);
                }
            }

            //low pass filter
            F += (F_pre_ - F) * exp(-1.0 / t_const_);
            for (unsigned int j=0; j<6; j++) {
                F_pre_(j) = 0;
            }
            F_pre_ += F;

            //tf transformation
            tf::vectorKDLToTF(F.force, tf_force);
            tf::vectorKDLToTF(F.torque, tf_torque);
            try {
                listener_.waitForTransform( tip, root, state->header.stamp, ros::Duration(1.0));
                listener_.lookupTransform( tip, root, state->header.stamp , transform);
            }
            catch (tf::TransformException ex) {
                ROS_ERROR("%s",ex.what());
                ros::Duration(1.0).sleep();
            }
            for (unsigned int j=0; j<3; j++) {
//.........这里部分代码省略.........

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

int main()
{
    Vector Fend(3); Fend.zero();
    Vector Muend(Fend);

    Vector w0(3); Vector dw0(3); Vector ddp0(3);
    w0 = dw0=ddp0=0.0; ddp0[1]=g;

    int N = 7;
    
    Vector q(N);
    Vector dq(N);
    Vector ddq(N);
    
    
    Random rng;
    rng.seed(yarp::os::Time::now());
    for(int i=0;i<N-1;i++) 
    {
            q[i] = 0*CTRL_DEG2RAD*rng.uniform();
            dq[i] = 0*CTRL_DEG2RAD*rng.uniform();
            ddq[i] = 0*CTRL_DEG2RAD*rng.uniform();
    }
    
    
    for(int i=N-1;i<N;i++) 
    {
            q[i] = 0;
            dq[i] = 0;
            ddq[i] = 1;
    }
    
     
     
    
    
    L5PMDyn threeChain;
    
    
    iDynInvSensorL5PM threeChainSensor(threeChain.asChain(),"papare",DYNAMIC,iCub::skinDynLib::VERBOSE);
    int sensor_link = threeChainSensor.getSensorLink();
    
    q = threeChain.setAng(q);
    dq = threeChain.setDAng(dq);
    ddq = threeChain.setD2Ang(ddq);
    
    int nj=N;
    KDL::JntArray jointpositions = KDL::JntArray(nj);
    KDL::JntArray jointvel = KDL::JntArray(nj);
    KDL::JntArray jointacc = KDL::JntArray(nj);
    KDL::JntArray torques = KDL::JntArray(nj);

    
    for(unsigned int i=0;i<nj;i++){
        jointpositions(i)=q[i];
        jointvel(i) = dq[i];
        jointacc(i) = ddq[i];
    }
    
    threeChain.prepareNewtonEuler(DYNAMIC);
    threeChain.computeNewtonEuler(w0,dw0,ddp0,Fend,Muend);
    threeChainSensor.computeSensorForceMoment();


    // then we can retrieve our results...
    // forces moments and torques
    Matrix F = threeChain.getForces();
    Matrix Mu = threeChain.getMoments();
    Vector Tau = threeChain.getTorques();
    
    Matrix F_KDL = idynChainGetForces_usingKDL(threeChain,ddp0);
    Matrix Mu_KDL = idynChainGetMoments_usingKDL(threeChain,ddp0);
    Vector Tau_KDL = idynChainGetTorques_usingKDL(threeChain,ddp0);
    
    Matrix F_KDL_sens = idynChainGetForces_usingKDL(threeChain,threeChainSensor,ddp0);
    Matrix Mu_KDL_sens = idynChainGetMoments_usingKDL(threeChain,threeChainSensor,ddp0);
    Vector Tau_KDL_sens = idynChainGetTorques_usingKDL(threeChain,threeChainSensor,ddp0);
    
    Matrix p_idyn(3,N),p_kdl_no_sens(3,N),p_kdl_sens(3,N+1);
    
    for(int l=0;l<N;l++) 
    {
        p_idyn.setCol(l,threeChain.getH(l).subcol(0,3,3));
    }
    KDL::Chain threeChainKDL;
    
    std::vector<std::string> dummy;
    std::string dummy_str = "";
    
    for(int l=0;l<N;l++) {
        Vector p_kdl;
        idynChain2kdlChain(*(threeChain.asChain()),threeChainKDL,dummy,dummy,dummy_str,dummy_str,l+1);
        KDL::ChainFkSolverPos_recursive posSolver = KDL::ChainFkSolverPos_recursive(threeChainKDL);
        KDL::Frame cartpos;
        KDL::JntArray jpos;
        jpos.resize(l+1);
        for(int p=0;p<jpos.rows();p++) jpos(p) = jointpositions(p);
        posSolver.JntToCart(jpos,cartpos);
        to_iDyn(cartpos.p,p_kdl);
        p_kdl_no_sens.setCol(l,p_kdl);
//.........这里部分代码省略.........