本文整理汇总了C++中ColumnVector::Ncols方法的典型用法代码示例。如果您正苦于以下问题：C++ ColumnVector::Ncols方法的具体用法？C++ ColumnVector::Ncols怎么用？C++ ColumnVector::Ncols使用的例子？那么, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类ColumnVector的用法示例。
在下文中一共展示了ColumnVector::Ncols方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: dqp_torque
void Robot::dqp_torque(const ColumnVector & q, const ColumnVector & qp,
const ColumnVector & dqp,
ColumnVector & ltorque, ColumnVector & dtorque)
{
int i;
ColumnVector z0(3);
Matrix Rt, temp;
Matrix Q(3,3);
ColumnVector *w, *wp, *vp, *a, *f, *n, *F, *N, *p;
ColumnVector *dw, *dwp, *dvp, *da, *df, *dn, *dF, *dN, *dp;
if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
ltorque = ColumnVector(dof);
dtorque = ColumnVector(dof);
set_q(q);
w = new ColumnVector[dof+1];
wp = new ColumnVector[dof+1];
vp = new ColumnVector[dof+1];
a = new ColumnVector[dof+1];
f = new ColumnVector[dof+1];
n = new ColumnVector[dof+1];
F = new ColumnVector[dof+1];
N = new ColumnVector[dof+1];
p = new ColumnVector[dof+1];
dw = new ColumnVector[dof+1];
dwp = new ColumnVector[dof+1];
dvp = new ColumnVector[dof+1];
da = new ColumnVector[dof+1];
df = new ColumnVector[dof+1];
dn = new ColumnVector[dof+1];
dF = new ColumnVector[dof+1];
dN = new ColumnVector[dof+1];
dp = new ColumnVector[dof+1];
w[0] = ColumnVector(3);
wp[0] = ColumnVector(3);
vp[0] = gravity;
dw[0] = ColumnVector(3);
dwp[0] = ColumnVector(3);
dvp[0] = ColumnVector(3);
z0 = 0.0;
Q = 0.0;
Q(1,2) = -1.0;
Q(2,1) = 1.0;
z0(3) = 1.0;
w[0] = 0.0;
wp[0] = 0.0;
dw[0] = 0.0;
dwp[0] = 0.0;
dvp[0] = 0.0;
for(i = 1; i <= dof; i++) {
Rt = links[i].R.t();
p[i] = ColumnVector(3);
p[i](1) = links[i].get_a();
p[i](2) = links[i].get_d() * Rt(2,3);
p[i](3) = links[i].get_d() * Rt(3,3);
if(links[i].get_joint_type() != 0) {
dp[i] = ColumnVector(3);
dp[i](1) = 0.0;
dp[i](2) = Rt(2,3);
dp[i](3) = Rt(3,3);
}
if(links[i].get_joint_type() == 0) {
w[i] = Rt*(w[i-1] + z0*qp(i));
dw[i] = Rt*(dw[i-1] + z0*dqp(i));
wp[i] = Rt*(wp[i-1] + vec_x_prod(w[i-1],z0*qp(i)));
dwp[i] = Rt*(dwp[i-1]
+ vec_x_prod(dw[i-1],z0*qp(i))
+ vec_x_prod(w[i-1],z0*dqp(i))
);
vp[i] = vec_x_prod(wp[i],p[i])
+ vec_x_prod(w[i],vec_x_prod(w[i],p[i]))
+ Rt*(vp[i-1]);
dvp[i] = vec_x_prod(dwp[i],p[i])
+ vec_x_prod(dw[i],vec_x_prod(w[i],p[i]))
+ vec_x_prod(w[i],vec_x_prod(dw[i],p[i]))
+ Rt*dvp[i-1];
} else {
w[i] = Rt*w[i-1];
dw[i] = Rt*dw[i-1];
wp[i] = Rt*wp[i-1];
dwp[i] = Rt*dwp[i-1];
vp[i] = Rt*(vp[i-1]
+ vec_x_prod(w[i],z0*qp(i))) * 2.0
+ vec_x_prod(wp[i],p[i])
+ vec_x_prod(w[i],vec_x_prod(w[i],p[i]));
dvp[i] = Rt*(dvp[i-1]
+ (vec_x_prod(dw[i],z0*qp(i)) * 2.0
+ vec_x_prod(w[i],z0*dqp(i))))
+ vec_x_prod(dwp[i],p[i])
+ vec_x_prod(dw[i],vec_x_prod(w[i],p[i]))
+ vec_x_prod(w[i],vec_x_prod(dw[i],p[i]));
}
a[i] = vec_x_prod(wp[i],links[i].r)
+ vec_x_prod(w[i],vec_x_prod(w[i],links[i].r))
+ vp[i];
da[i] = vec_x_prod(dwp[i],links[i].r)
+ vec_x_prod(dw[i],vec_x_prod(w[i],links[i].r))
+ vec_x_prod(w[i],vec_x_prod(dw[i],links[i].r))
+ dvp[i];
}
//.........这里部分代码省略.........