当前位置: 首页>>代码示例>>C++>>正文


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

本文整理汇总了C++中JntArray::rows方法的典型用法代码示例。如果您正苦于以下问题:C++ JntArray::rows方法的具体用法?C++ JntArray::rows怎么用?C++ JntArray::rows使用的例子?那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在JntArray的用法示例。


在下文中一共展示了JntArray::rows方法的15个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: CartToJnt

    int ChainIkSolverPos_NR::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
    {
        if(nj != chain.getNrOfJoints())
            return (error = E_NOT_UP_TO_DATE);
        if(q_init.rows() != nj || q_out.rows() != nj)
            return (error = E_SIZE_MISMATCH);

            q_out = q_init;

            unsigned int i;
            for(i=0;i<maxiter;i++){
                if (E_NOERROR > fksolver.JntToCart(q_out,f) )
                    return (error = E_FKSOLVERPOS_FAILED);
                delta_twist = diff(f,p_in);
                const int rc = iksolver.CartToJnt(q_out,delta_twist,delta_q);
                if (E_NOERROR > rc)
                    return (error = E_IKSOLVER_FAILED);
                // we chose to continue if the child solver returned a positive
                // "error", which may simply indicate a degraded solution
                Add(q_out,delta_q,q_out);
                if(Equal(delta_twist,Twist::Zero(),eps))
                    // converged, but possibly with a degraded solution
                    return (rc > E_NOERROR ? E_DEGRADED : E_NOERROR);
            }
            return (error = E_MAX_ITERATIONS_EXCEEDED);        // failed to converge
    }
开发者ID:snrkiwi,项目名称:orocos_kinematics_dynamics,代码行数:26,代码来源:chainiksolverpos_nr.cpp

示例2: CartToJnt

int TreeIdSolver_Vereshchagin::CartToJnt(const JntArray& q, const JntArray& q_dot, JntArray& q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, JntArray &torques)
{
    //Check sizes always
    if (q.rows() != nj || q_dot.rows() != nj || q_dotdot.rows() != nj || torques.rows() != nj)// || f_ext.size() != ns)
    {
        std::cout << "Error 1" << std::endl;
        return -1;
    }
    if (alfa.columns() != nc || beta.rows() != nc)
        //  if (alfas.size() != nc || betas.size() != nc) // number of constraints should also become a vector. because nc defines a number of columns in constraint jacobian matrix A(alfa)
    {
        std::cout << "Error 2" << std::endl;
        return -2;
    }

    //do an upward recursion for position and velocities
    this->initial_upwards_sweep(q, q_dot, q_dotdot, f_ext);
    //do an inward recursion for inertia, forces and constraints
    this->downwards_sweep(alfa, torques);
    //Solve for the constraint forces
    //    this->constraint_calculation(beta);
    //do an upward recursion to propagate the result
    //    this->final_upwards_sweep(q_dotdot, torques);
    return 0;
}
开发者ID:shakhimardanov,项目名称:orocos_kdl_diamondback,代码行数:25,代码来源:treeidsolver_vereshchagin.cpp

示例3: assert

inline Twist operator*(const Jacobian& jac, const JntArray& qdot)
{
  assert(jac.columns() == qdot.rows());
  Twist t;

  int naxes = qdot.rows();
  for(int i=0; i < 6; ++i)
    for(int j=0; j < naxes; ++j)
      t(i) += jac(i,j)*qdot(j);

  return t;
}
开发者ID:airballking,项目名称:feature_constraints_controller,代码行数:12,代码来源:ChainTask.cpp

示例4: CartToJnt

    int ChainIdSolver_RNE::CartToJnt(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext,JntArray &torques)
    {
        //Check sizes when in debug mode
        if(q.rows()!=nj || q_dot.rows()!=nj || q_dotdot.rows()!=nj || torques.rows()!=nj || f_ext.size()!=ns)
            return -1;
        unsigned int j=0;

        //Sweep from root to leaf
        for(unsigned int i=0;i<ns;i++){
            double q_,qdot_,qdotdot_;
            if(chain.getSegment(i).getJoint().getType()!=Joint::None){
                q_=q(j);
                qdot_=q_dot(j);
                qdotdot_=q_dotdot(j);
                j++;
            }else
                q_=qdot_=qdotdot_=0.0;

            //Calculate segment properties: X,S,vj,cj
            X[i]=chain.getSegment(i).pose(q_);//Remark this is the inverse of the
                                                //frame for transformations from
                                                //the parent to the current coord frame
            //Transform velocity and unit velocity to segment frame
            Twist vj=X[i].M.Inverse(chain.getSegment(i).twist(q_,qdot_));
            S[i]=X[i].M.Inverse(chain.getSegment(i).twist(q_,1.0));
            //We can take cj=0, see remark section 3.5, page 55 since the unit velocity vector S of our joints is always time constant
            //calculate velocity and acceleration of the segment (in segment coordinates)
            if(i==0){
                v[i]=vj;
                a[i]=X[i].Inverse(ag)+S[i]*qdotdot_+v[i]*vj;
            }else{
                v[i]=X[i].Inverse(v[i-1])+vj;
                a[i]=X[i].Inverse(a[i-1])+S[i]*qdotdot_+v[i]*vj;
            }
            //Calculate the force for the joint
            //Collect RigidBodyInertia and external forces
            RigidBodyInertia Ii=chain.getSegment(i).getInertia();
            f[i]=Ii*a[i]+v[i]*(Ii*v[i])-f_ext[i];
            //std::cout << "a[i]=" << a[i] << "\n f[i]=" << f[i] << "\n S[i]" << S[i] << std::endl;
        }
        //Sweep from leaf to root
        j=nj-1;
        for(int i=ns-1;i>=0;i--){
            if(chain.getSegment(i).getJoint().getType()!=Joint::None)
                torques(j--)=dot(S[i],f[i]);
            if(i!=0)
                f[i-1]=f[i-1]+X[i]*f[i];
        }
        return 0;
    }
开发者ID:Jeremywoo,项目名称:FreeCAD,代码行数:50,代码来源:chainidsolver_recursive_newton_euler.cpp

示例5: setOptPos

 int ChainIkSolverVel_pinv_nso::setOptPos(const JntArray & _opt_pos)
 {
     if (nj != _opt_pos.rows())
         return (error = E_SIZE_MISMATCH);
   opt_pos = _opt_pos;
   return (error = E_NOERROR);
 }
开发者ID:orocos,项目名称:orocos_kinematics_dynamics,代码行数:7,代码来源:chainiksolvervel_pinv_nso.cpp

示例6: setWeights

 int ChainIkSolverVel_pinv_nso::setWeights(const JntArray & _weights)
 {
     if (nj != _weights.rows())
         return (error = E_SIZE_MISMATCH);
   weights = _weights;
   return (error = E_NOERROR);
 }
开发者ID:orocos,项目名称:orocos_kinematics_dynamics,代码行数:7,代码来源:chainiksolvervel_pinv_nso.cpp

示例7: diff

JntArray diff(const JntArray& q,const JntArray& qdot,const double& dt)
{
    JntArray q_qdqt(q);
    for(int i=0; i<q.rows(); i++)
        q_qdqt(i) += dt*qdot(i);
    return q_qdqt;
}
开发者ID:JavierIH,项目名称:orocos_kinematics_dynamics,代码行数:7,代码来源:jacobiandottest.cpp

示例8: CartToJnt

int ChainIkSolverPos_NR_JL_Mimic::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
{
  //Note that q_init and q_out will be of size chain.getNrOfJoints() - num_mimic_joints
  qToqMimic(q_init,q_temp);  

  unsigned int i;
  for(i=0;i<maxiter;++i)
  {
    fksolver.JntToCart(q_temp,f);
    delta_twist = diff(f,p_in);

    //    if(Equal(delta_twist,Twist::Zero(),eps))
    //      break;
    if(fabs(delta_twist(0)) < eps && fabs(delta_twist(1)) < eps && fabs(delta_twist(2)) < eps)
       break;

    ROS_DEBUG_STREAM("delta_twist");
    for(std::size_t i=0; i < 6; ++i)
      ROS_DEBUG("%d: %f",(int) i, delta_twist(i));
    
    iksolver.CartToJnt(q_temp,delta_twist,delta_q);
    Add(q_temp,delta_q,q_temp);

    ROS_DEBUG_STREAM("delta_q");
    for(std::size_t i=0; i < delta_q.rows(); ++i)
      ROS_DEBUG("%d: %f",(int) i, delta_q(i));
    
    for(std::size_t j=0; j<q_min_mimic.rows(); ++j) 
    {
      if(mimic_joints[j].active)
        if(q_temp(j) < q_min_mimic(j))
          q_temp(j) = q_min_mimic(j);
    }    
    for(std::size_t j=0; j<q_max_mimic.rows(); ++j) 
    {
      if(mimic_joints[j].active)
        if(q_temp(j) > q_max_mimic(j))
          q_temp(j) = q_max_mimic(j);
    }

    //Make sure limits are applied on the mimic joints to
    qMimicToq(q_temp,q_out);
    qToqMimic(q_out,q_temp);    

  }
  
  qMimicToq(q_temp, q_out);  
  ROS_DEBUG_STREAM("Full Solution:");
  for(std::size_t i=0; i < q_temp.rows(); ++i)
    ROS_DEBUG("%d: %f",(int) i,q_temp(i));

  ROS_DEBUG_STREAM("Actual Solution:");
  for(std::size_t i=0; i < q_out.rows(); ++i)
    ROS_DEBUG("%d: %f",(int) i,q_out(i));

  if(i!=maxiter)
    return 0;
  else
    return -3;
}
开发者ID:mpomarlan,项目名称:moveit_puzzle_demo,代码行数:60,代码来源:chainiksolver_pos_nr_jl_mimic.cpp

示例9: CartToJnt

double TreeIkSolverPos_Online::CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
{
  assert(q_out.rows() == q_in.rows());
  assert(q_dot_.rows() == q_out.rows());

  q_out = q_in;

  // First check, if all elements in p_in are available
  for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
    if(frames_.find(f_des_it->first)==frames_.end())
      return -2;

  for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
  {
    // Get all iterators for this endpoint
    Frames::iterator f_it = frames_.find(f_des_it->first);
    Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);

    fksolver_.JntToCart(q_out, f_it->second, f_it->first);
    twist_ = diff(f_it->second, f_des_it->second);

    // Checks, if the twist (twist_) exceeds the maximum translational and/or rotational velocity
    // And scales them, if necessary
    enforceCartVelLimits();

    delta_twists_it->second = twist_;
  }

  double res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);

  // Checks, if joint velocities (q_dot_) exceed their maximum velocities and scales them, if necessary
  enforceJointVelLimits();

  // Integrate
  Add(q_out, q_dot_, q_out);

  // Limit joint positions
  for (unsigned int j = 0; j < q_min_.rows(); j++)
  {
    if (q_out(j) < q_min_(j))
      q_out(j) = q_min_(j);
    else if (q_out(j) > q_max_(j))
      q_out(j) = q_max_(j);
  }

  return res;
}
开发者ID:bit-pirate,项目名称:reem_teleop,代码行数:47,代码来源:treeiksolverpos_online.cpp

示例10: CartToJnt

 double TreeIkSolverVel_wdls::CartToJnt(const JntArray& q_in, const Twists& v_in, JntArray& qdot_out) {
     
     //First check if we are configured for this Twists:
     for (Twists::const_iterator v_it = v_in.begin(); v_it != v_in.end(); ++v_it) {
         if (jacobians.find(v_it->first) == jacobians.end())
             return -2;
     }
     //Check if q_in has the right size
     if (q_in.rows() != tree.getNrOfJoints())
         return -1;
     
     //Lets get all the jacobians we need:
     unsigned int k = 0;
     for (Jacobians::iterator jac_it = jacobians.begin(); jac_it
              != jacobians.end(); ++jac_it) {
         int ret = jnttojacsolver.JntToJac(q_in, jac_it->second, jac_it->first);
         if (ret < 0)
             return ret;
         else {
             //lets put the jacobian in the big matrix and put the twist in the big t:
             J.block(6*k,0, 6,tree.getNrOfJoints()) = jac_it->second.data;
             const Twist& twist=v_in.find(jac_it->first)->second;
             t.segment(6*k,3)   = Eigen::Map<Eigen::Vector3d>(twist.vel.data);
             t.segment(6*k+3,3) = Eigen::Map<Eigen::Vector3d>(twist.rot.data);
         }
         ++k;
     }
     
     //Lets use the wdls algorithm to find the qdot:
     // Create the Weighted jacobian
     J_Wq = (J * Wq).lazy();
     Wy_J_Wq = (Wy * J_Wq).lazy();
     
     // Compute the SVD of the weighted jacobian
     int ret = svd_eigen_HH(Wy_J_Wq, U, S, V, tmp);
     
     //Pre-multiply U and V by the task space and joint space weighting matrix respectively
     Wy_t = (Wy * t).lazy();
     Wq_V = (Wq * V).lazy();
     
     // tmp = (Si*Wy*U'*y),
     for (unsigned int i = 0; i < J.cols(); i++) {
         double sum = 0.0;
         for (unsigned int j = 0; j < J.rows(); j++) {
             if (i < Wy_t.size())
                 sum += U(j, i) * Wy_t(j);
             else
                 sum += 0.0;
         }
         tmp( i) = sum * ((S(i) / (S(i) * S(i) + lambda * lambda)));
     }
     
     // x = Lx^-1*V*tmp + x
     qdot_out.data = (Wq_V * tmp).lazy();
     
     return Wy_t.norm();
 }
开发者ID:Barleyman,项目名称:FreeCAD_sf_master,代码行数:57,代码来源:treeiksolvervel_wdls.cpp

示例11: JntToJac

int TreeJntToJacSolver::JntToJac(const JntArray& q_in, Jacobian& jac,
        const std::string& segmentname) {
    //First we check all the sizes:
    if (q_in.rows() != tree.getNrOfJoints() || jac.columns()
            != tree.getNrOfJoints())
        return -1;

    //Lets search the tree-element
    SegmentMap::value_type const* it = tree.getSegmentPtr(segmentname);

    //If segmentname is not inside the tree, back out:
    if (!it)
        return -2;

    //Let's make the jacobian zero:
    SetToZero(jac);

    SegmentMap::value_type const* root = tree.getSegmentPtr("root");

    Frame T_total = Frame::Identity();
	Frame T_local, T_joint;
	Twist t_local;
    //Lets recursively iterate until we are in the root segment
    while (it != root) {
        //get the corresponding q_nr for this TreeElement:
        unsigned int q_nr = it->second.q_nr;

        //get the pose of the joint.
		T_joint = it->second.segment.getJoint().pose(((JntArray&)q_in)(q_nr));
		// combine with the tip to have the tip pose
		T_local = T_joint*it->second.segment.getFrameToTip();
        //calculate new T_end:
        T_total = T_local * T_total;

        //get the twist of the segment:
		int ndof = it->second.segment.getJoint().getNDof();
		for (int dof=0; dof<ndof; dof++) {
			// combine joint rotation with tip position to get a reference frame for the joint
			T_joint.p = T_local.p;
			// in which the twist can be computed (needed for NDof joint)
            t_local = it->second.segment.twist(T_joint, 1.0, dof);
            //transform the endpoint of the local twist to the global endpoint:
            t_local = t_local.RefPoint(T_total.p - T_local.p);
            //transform the base of the twist to the endpoint
            t_local = T_total.M.Inverse(t_local);
            //store the twist in the jacobian:
            jac.twists[q_nr+dof] = t_local;
        }
        //goto the parent
        it = it->second.parent;
    }//endwhile
    //Change the base of the complete jacobian from the endpoint to the base
    changeBase(jac, T_total.M, jac);

    return 0;

}//end JntToJac
开发者ID:dfelinto,项目名称:blender,代码行数:57,代码来源:treejnttojacsolver.cpp

示例12: Jdot_d2_symbolic

Jacobian Jdot_d2_symbolic(const JntArray& q,const JntArray& qdot)
{
    // Returns Jdot for the simple 2DOF arm 
    Jacobian Jdot(q.rows());
    SetToZero(Jdot);
    Jdot(0,0) =  -L1 * (qdot(0) + qdot(1))*cos(q(0)+q(1))-L0*cos(q(0))*qdot(0);
    Jdot(0,1) =  -L1 * (qdot(0) + qdot(1))*cos(q(0)+q(1));
    Jdot(1,0) =  -L1 * (qdot(0) + qdot(1))*sin(q(0)+q(1))-L0*sin(q(0))*qdot(0);
    Jdot(1,1) =  -L1 * (qdot(0) + qdot(1))*sin(q(0)+q(1));
    return Jdot;
}
开发者ID:JavierIH,项目名称:orocos_kinematics_dynamics,代码行数:11,代码来源:jacobiandottest.cpp

示例13: GetNextJointArray

int JointStatesGetter::GetNextJointArray(JntArray& joint_array)
{
  if (infile_.eof())
    return -1 ;

  const unsigned int N = joint_array.rows() ;
  
  for (unsigned int i=0; i<N; i++)
    infile_ >> joint_array(i) ;
  
  return 0 ;
}
开发者ID:goretkin,项目名称:kwc-ros-pkg,代码行数:12,代码来源:verify_jacobian.cpp

示例14: J_d2_symbolic

Jacobian J_d2_symbolic(const JntArray& q,const JntArray& qdot)
{
    // Returns J for the simple 2DOF arm
    Jacobian J(q.rows());
    SetToZero(J);
    J(0,0) =  -L1 * sin(q(0)+q(1))-L0*sin(q(0));
    J(0,1) =  -L1 * sin(q(0)+q(1));
    J(1,0) =   L1 * cos(q(0)+q(1))+L0*cos(q(0));
    J(1,1) =   L1 * cos(q(0)+q(1));
    J(5,0) = J(5,1) = 1;
    return J;
}
开发者ID:JavierIH,项目名称:orocos_kinematics_dynamics,代码行数:12,代码来源:jacobiandottest.cpp

示例15: JntToCart

    int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName)
    {      
		SegmentMap::const_iterator it = tree.getSegment(segmentName); 
       
        
        if(q_in.rows() != tree.getNrOfJoints())
    	    	return -1;
        else if(it == tree.getSegments().end()) //if the segment name is not found
         	return -2;
        else{
			p_out = recursiveFk(q_in, it);
        	return 0;        	
        }
    }
开发者ID:3DPrinterGuy,项目名称:FreeCAD,代码行数:14,代码来源:treefksolverpos_recursive.cpp


注:本文中的JntArray::rows方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。