本文整理汇总了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
}
示例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;
}
示例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;
}
示例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;
}
示例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);
}
示例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);
}
示例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;
}
示例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;
}
示例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;
}
示例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();
}
示例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
示例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;
}
示例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 ;
}
示例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;
}
示例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;
}
}