54 static const char rcsid[] =
"$Id: delta_t.cpp,v 1.17 2005/07/01 16:11:45 gourdeau Exp $";
61 using namespace NEWMAT;
159 if(q.
Ncols() != 1 || q.
Nrows() != dof) error(
"q has wrong dimension");
160 if(qp.
Ncols() != 1 || qp.
Nrows() != dof) error(
"qp has wrong dimension");
161 if(qpp.
Ncols() != 1 || qpp.
Nrows() != dof) error(
"qpp has wrong dimension");
162 if(dq.
Ncols() != 1 || dq.
Nrows() != dof) error(
"dq has wrong dimension");
163 if(dqp.
Ncols() != 1 || dqp.
Nrows() != dof) error(
"dqp has wrong dimension");
164 if(dqpp.
Ncols() != 1 || dqpp.
Nrows() != dof) error(
"dqpp has wrong dimension");
171 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
176 for(i = 1; i <= dof; i++)
180 p[i](1) = links[i].get_a();
181 p[i](2) = links[i].get_d() * Rt(2,3);
182 p[i](3) = links[i].get_d() * Rt(3,3);
183 if(links[i].get_joint_type() != 0)
190 if(links[i].get_joint_type() == 0)
192 w[i] = Rt*(w[i-1] + z0*qp(i));
193 dw[i] = Rt*(dw[i-1] + z0*dqp(i)
194 - Q*(w[i-1] + z0*qp(i))*dq(i));
195 wp[i] = Rt*(wp[i-1] + z0*qpp(i)
197 dwp[i] = Rt*(dwp[i-1] + z0*dqpp(i)
208 + Rt*(dvp[i-1] - Q*vp[i-1]*dq(i));
215 dwp[i] = Rt*dwp[i-1];
216 vp[i] = Rt*(vp[i-1] + z0*qpp(i)
220 dvp[i] = Rt*(dvp[i-1] + z0*dqpp(i)
239 for(i = dof; i >= 1; i--)
241 F[i] =
a[i] * links[i].m;
242 dF[i] = da[i] * links[i].m;
243 N[i] = links[i].I*wp[i] +
CrossProduct(w[i],links[i].I*w[i]);
244 dN[i] = links[i].I*dwp[i] +
CrossProduct(dw[i],links[i].I*w[i])
254 if(links[i].get_joint_type() != 0)
259 f[i] = links[i+1].R*f[i+1] + F[i];
260 df[i] = links[i+1].R*df[i+1] + dF[i];
261 if(links[i].get_joint_type() == 0)
262 df[i] += Q*links[i+1].R*f[i+1]*dq(i+1);
267 if(links[i].get_joint_type() == 0)
268 dn[i] += Q*links[i+1].R*n[i+1]*dq(i+1);
273 if(links[i].get_joint_type() == 0)
275 temp = ((z0.
t()*links[i].R)*n[i]);
276 ltorque(i) = temp(1,1);
277 temp = ((z0.
t()*links[i].R)*dn[i]);
278 dtorque(i) = temp(1,1);
282 temp = ((z0.
t()*links[i].R)*f[i]);
283 ltorque(i) = temp(1,1);
284 temp = ((z0.
t()*links[i].R)*df[i]);
285 dtorque(i) = temp(1,1);
387 if(q.
Ncols() != 1 || q.
Nrows() != dof) error(
"q has wrong dimension");
388 if(qp.
Ncols() != 1 || qp.
Nrows() != dof) error(
"qp has wrong dimension");
389 if(qpp.
Ncols() != 1 || qpp.
Nrows() != dof) error(
"qpp has wrong dimension");
390 if(dq.
Ncols() != 1 || dq.
Nrows() != dof) error(
"dq has wrong dimension");
391 if(dqp.
Ncols() != 1 || dqp.
Nrows() != dof) error(
"dqp has wrong dimension");
392 if(dqpp.
Ncols() != 1 || dqpp.
Nrows() != dof) error(
"dqpp has wrong dimension");
399 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
404 for(i = 1; i <= dof; i++)
408 if(links[i].get_joint_type() != 0)
415 if(links[i].get_joint_type() == 0)
417 w[i] = Rt*w[i-1] + z0*qp(i);
418 dw[i] = Rt*dw[i-1] + z0*dqp(i)
422 dwp[i] = Rt*dwp[i-1] +
CrossProduct(Rt*dw[i-1],z0*qp(i))
424 - (Q*Rt*wp[i-1] +
CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
440 dwp[i] = Rt*dwp[i-1];
462 for(i = dof; i >= 1; i--) {
463 F[i] =
a[i] * links[i].m;
464 N[i] = links[i].I*wp[i] +
CrossProduct(w[i],links[i].I*w[i]);
465 dF[i] = da[i] * links[i].m;
466 dN[i] = links[i].I*dwp[i] +
CrossProduct(dw[i],links[i].I*w[i])
478 f[i] = links[i+1].R*f[i+1] + F[i];
479 df[i] = links[i+1].R*df[i+1] + dF[i];
480 if(links[i+1].get_joint_type() == 0)
481 df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
483 n[i] = links[i+1].R*n[i+1] +
CrossProduct(p[i+1],links[i+1].R*f[i+1])
485 dn[i] = links[i+1].R*dn[i+1] +
CrossProduct(p[i+1],links[i+1].R*df[i+1])
487 if(links[i+1].get_joint_type() == 0)
488 dn[i] += (links[i+1].R*Q*n[i+1] +
491 dn[i] +=
CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
494 if(links[i].get_joint_type() == 0)
497 ltorque(i) = temp(1,1);
499 dtorque(i) = temp(1,1);
504 ltorque(i) = temp(1,1);
506 dtorque(i) = temp(1,1);
523 if(q.
Ncols() != 1 || q.
Nrows() != dof) error(
"q has wrong dimension");
524 if(qp.
Ncols() != 1 || qp.
Nrows() != dof) error(
"qp has wrong dimension");
525 if(qpp.
Ncols() != 1 || qpp.
Nrows() != dof) error(
"qpp has wrong dimension");
526 if(dq.
Ncols() != 1 || dq.
Nrows() != dof) error(
"dq has wrong dimension");
527 if(dqp.
Ncols() != 1 || dqp.
Nrows() != dof) error(
"dqp has wrong dimension");
528 if(dqpp.
Ncols() != 1 || dqpp.
Nrows() != dof) error(
"dqpp has wrong dimension");
535 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
540 for(i = 1; i <= dof; i++)
544 if(links[i].get_joint_type() != 0)
551 if(links[i].get_joint_type() == 0)
553 w[i] = Rt*w[i-1] + z0*qp(i);
554 dw[i] = Rt*dw[i-1] + z0*dqp(i)
558 dwp[i] = Rt*dwp[i-1] +
CrossProduct(Rt*dw[i-1],z0*qp(i))
560 - (Q*Rt*wp[i-1] +
CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
576 dwp[i] = Rt*dwp[i-1];
590 for(i = dof; i >= 1; i--) {
591 F[i] = vp[i]*links[i].m +
CrossProduct(wp[i], links[i].mc) +
593 dF[i] = dvp[i]*links[i].m +
CrossProduct(dwp[i],links[i].mc)
596 N[i] = links[i].I*wp[i] +
CrossProduct(w[i],links[i].I*w[i])
598 dN[i] = links[i].I*dwp[i] +
CrossProduct(dw[i],links[i].I*w[i])
611 f[i] = links[i+1].R*f[i+1] + F[i];
612 df[i] = links[i+1].R*df[i+1] + dF[i];
613 if(links[i+1].get_joint_type() == 0)
614 df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
616 n[i] = links[i+1].R*n[i+1] +
CrossProduct(p[i+1],links[i+1].R*f[i+1])
618 dn[i] = links[i+1].R*dn[i+1] +
CrossProduct(p[i+1],links[i+1].R*df[i+1])
620 if(links[i+1].get_joint_type() == 0)
621 dn[i] += (links[i+1].R*Q*n[i+1] +
624 dn[i] +=
CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
627 if(links[i].get_joint_type() == 0)
630 ltorque(i) = temp(1,1);
632 dtorque(i) = temp(1,1);
637 ltorque(i) = temp(1,1);
639 dtorque(i) = temp(1,1);
static const char rcsid[]
RCS/CVS version.
Robots class definitions.
virtual void delta_torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, const ColumnVector &dqp, const ColumnVector &dqpp, ColumnVector &torque, ColumnVector &dtorque)
Delta torque dynamics.
virtual void delta_torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, const ColumnVector &dqp, const ColumnVector &dqpp, ColumnVector &torque, ColumnVector &dtorque)
Delta torque dynamics.
TransposedMatrix t() const
Matrix CrossProduct(const Matrix &A, const Matrix &B)
The usual rectangular matrix.
FloatVector FloatVector * a
virtual void delta_torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, const ColumnVector &dqp, const ColumnVector &dqpp, ColumnVector <orque, ColumnVector &dtorque)
Delta torque dynamics.