72 static const char rcsid[] =
"$Id: dynamics.cpp,v 1.34 2006/05/19 18:32:30 gourdeau Exp $";
79 using namespace NEWMAT;
89 for(
int i = 1; i <= dof; i++) {
90 for(
int j = 1; j <= dof; j++) {
91 torque(j) = (i == j ? 1.0 : 0.0);
93 torque = torque_novelocity(torque);
107 qpp = inertia(q).
i()*(tau_cmd-torque(q,qp,qpp));
130 qpp = inertia(q).
i()*(tau_cmd-torque(q, qp, qpp, Fext, Next));
145 return torque(q, qp, qpp, Fext, Next);
219 if(q.
Nrows() != dof) error(
"q has wrong dimension");
220 if(qp.
Nrows() != dof) error(
"qp has wrong dimension");
221 if(qpp.
Nrows() != dof) error(
"qpp has wrong dimension");
226 for(i = 1; i <= dof; i++) {
228 if(links[i].get_joint_type() == 0) {
229 w[i] = Rt*(w[i-1] + z0*qp(i));
230 wp[i] = Rt*(wp[i-1] + z0*qpp(i)
238 vp[i] = Rt*(vp[i-1] + z0*qpp(i))
248 for(i = dof; i >= 1; i--) {
249 F[i] =
a[i] * links[i].m;
250 N[i] = links[i].I*wp[i] +
CrossProduct(w[i],links[i].I*w[i]);
256 f[i] = links[i+1].R*f[i+1] + F[i];
260 if(links[i].get_joint_type() == 0)
261 temp = ((z0.t()*links[i].R)*n[i]);
263 temp = ((z0.t()*links[i].R)*f[i]);
264 ltorque(i) = temp(1,1)
265 + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
266 + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*
sign(qp(i)));
269 ltorque.
Release();
return ltorque;
281 if(qpp.
Nrows() != dof) error(
"qpp has wrong dimension");
284 for(i = 1; i <= dof; i++) {
286 if(links[i].get_joint_type() == 0) {
287 wp[i] = Rt*(wp[i-1] + z0*qpp(i));
292 vp[i] = Rt*(vp[i-1] + z0*qpp(i))
298 for(i = dof; i >= 1; i--) {
299 F[i] =
a[i] * links[i].m;
300 N[i] = links[i].I*wp[i];
306 f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
307 n_nv[i] = links[i+1].R*n_nv[i+1] +
CrossProduct(p[i],f_nv[i])
310 if(links[i].get_joint_type() == 0)
311 temp = ((z0.t()*links[i].R)*n_nv[i]);
313 temp = ((z0.t()*links[i].R)*f_nv[i]);
314 ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
318 ltorque.
Release();
return ltorque;
329 for(i = 1; i <= dof; i++) {
331 if(links[i].get_joint_type() == 0)
332 vp[i] = Rt*(vp[i-1]);
339 for(i = dof; i >= 1; i--) {
340 F[i] =
a[i] * links[i].m;
346 f[i] = links[i+1].R*f[i+1] + F[i];
350 if(links[i].get_joint_type() == 0)
351 temp = ((z0.t()*links[i].R)*n[i]);
353 temp = ((z0.t()*links[i].R)*f[i]);
354 ltorque(i) = temp(1,1);
357 ltorque.
Release();
return ltorque;
366 if(qp.
Nrows() != dof) error(
"qp has wrong dimension");
369 for(i = 1; i <= dof; i++) {
371 if(links[i].get_joint_type() == 0) {
372 w[i] = Rt*(w[i-1] + z0*qp(i));
389 for(i = dof; i >= 1; i--) {
390 F[i] =
a[i] * links[i].m;
391 N[i] = links[i].I*wp[i] +
CrossProduct(w[i],links[i].I*w[i]);
397 f[i] = links[i+1].R*f[i+1] + F[i];
401 if(links[i].get_joint_type() == 0)
402 temp = ((z0.t()*links[i].R)*n[i]);
404 temp = ((z0.t()*links[i].R)*f[i]);
405 ltorque(i) = temp(1,1)
406 + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*
sign(qp(i)));
409 ltorque.
Release();
return ltorque;
419 return torque(q, qp, qpp, Fext, Next);
491 if(q.
Nrows() != dof) error(
"q has wrong dimension");
492 if(qp.
Nrows() != dof) error(
"qp has wrong dimension");
493 if(qpp.
Nrows() != dof) error(
"qpp has wrong dimension");
498 for(i = 1; i <= dof; i++) {
500 if(links[i].get_joint_type() == 0) {
501 w[i] = Rt*w[i-1] + z0*qp(i);
523 Fext = links[dof+fix].R*Fext_;
524 Next = links[dof+fix].R*Next_;
532 for(i = dof; i >= 1; i--) {
533 F[i] =
a[i] * links[i].m;
534 N[i] = links[i].I*wp[i] +
CrossProduct(w[i],links[i].I*w[i]);
539 f[i] = links[i+1].R*f[i+1] + F[i];
540 n[i] = links[i+1].R*n[i+1] +
CrossProduct(p[i+1],links[i+1].R*f[i+1])
543 if(links[i].get_joint_type() == 0)
544 temp = (z0.t()*n[i]);
546 temp = (z0.t()*f[i]);
547 ltorque(i) = temp(1,1)
548 + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
549 + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*
sign(qp(i)));
552 ltorque.
Release();
return ltorque;
561 if(qpp.
Ncols() != 1 || qpp.
Nrows() != dof) error(
"qpp has wrong dimension");
564 for(i = 1; i <= dof; i++) {
566 if(links[i].get_joint_type() == 0) {
567 wp[i] = Rt*wp[i-1] + z0*qpp(i);
577 for(i = dof; i >= 1; i--) {
578 F[i] =
a[i] * links[i].m;
579 N[i] = links[i].I*wp[i];
584 f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
585 n_nv[i] = links[i+1].R*n_nv[i+1] +
CrossProduct(p[i+1],links[i+1].R*f_nv[i+1])
589 if(links[i].get_joint_type() == 0)
590 temp = (z0.t()*n_nv[i]);
592 temp = (z0.t()*f_nv[i]);
593 ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
596 ltorque.
Release();
return ltorque;
607 for(i = 1; i <= dof; i++) {
609 if(links[i].get_joint_type() == 0)
616 for(i = dof; i >= 1; i--) {
617 F[i] =
a[i] * links[i].m;
622 f[i] = links[i+1].R*f[i+1] + F[i];
623 n[i] = links[i+1].R*n[i+1] +
CrossProduct(p[i+1],links[i+1].R*f[i+1])
626 if(links[i].get_joint_type() == 0)
627 temp = (z0.t()*n[i]);
629 temp = (z0.t()*f[i]);
630 ltorque(i) = temp(1,1);
633 ltorque.
Release();
return ltorque;
642 if(qp.
Nrows() != dof) error(
"qp has wrong dimension");
645 for(i = 1; i <= dof; i++) {
647 if(links[i].get_joint_type() == 0) {
648 w[i] = Rt*w[i-1] + z0*qp(i);
665 for(i = dof; i >= 1; i--) {
666 F[i] =
a[i] * links[i].m;
667 N[i] = links[i].I*wp[i] +
CrossProduct(w[i],links[i].I*w[i]);
672 f[i] = links[i+1].R*f[i+1] + F[i];
673 n[i] = links[i+1].R*n[i+1] +
CrossProduct(p[i+1],links[i+1].R*f[i+1])
676 if(links[i].get_joint_type() == 0)
677 temp = (z0.t()*n[i]);
679 temp = (z0.t()*f[i]);
680 ltorque(i) = temp(1,1)
681 + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*
sign(qp(i)));
684 ltorque.
Release();
return ltorque;
694 return torque(q, qp, qpp, Fext, Next);
712 if(q.
Nrows() != dof) error(
"q has wrong dimension");
713 if(qp.
Nrows() != dof) error(
"qp has wrong dimension");
714 if(qpp.
Nrows() != dof) error(
"qpp has wrong dimension");
719 for(i = 1; i <= dof; i++) {
721 if(links[i].get_joint_type() == 0)
723 w[i] = Rt*w[i-1] + z0*qp(i);
743 Fext = links[dof+fix].R*Fext_;
744 Next = links[dof+fix].R*Next_;
752 for(i = dof; i >= 1; i--)
754 F[i] = vp[i]*links[i].m +
CrossProduct(wp[i], links[i].mc) +
756 N[i] = links[i].I*wp[i] +
CrossProduct(w[i],links[i].I*w[i]) +
765 f[i] = links[i+1].R*f[i+1] + F[i];
766 n[i] = links[i+1].R*n[i+1] +
CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
769 if(links[i].get_joint_type() == 0)
770 temp = (z0.t()*n[i]);
772 temp = (z0.t()*f[i]);
773 ltorque(i) = temp(1,1)
774 + links[i].Im*links[i].Gr*links[i].Gr*qpp(i)
775 + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*
sign(qp(i)));
778 ltorque.
Release();
return ltorque;
788 if(qpp.
Ncols() != 1 || qpp.
Nrows() != dof) error(
"qpp has wrong dimension");
791 for(i = 1; i <= dof; i++)
794 if(links[i].get_joint_type() == 0)
796 wp[i] = Rt*wp[i-1] + z0*qpp(i);
807 for(i = dof; i >= 1; i--)
809 F[i] = vp[i]*links[i].m +
CrossProduct(wp[i], links[i].mc);
810 N[i] = links[i].I*wp[i] +
CrossProduct(-vp[i], links[i].mc);
818 f_nv[i] = links[i+1].R*f_nv[i+1] + F[i];
819 n_nv[i] = links[i+1].R*n_nv[i+1] +
CrossProduct(p[i+1],links[i+1].R*f_nv[i+1]) + N[i];
822 if(links[i].get_joint_type() == 0)
823 temp = (z0.t()*n_nv[i]);
825 temp = (z0.t()*f_nv[i]);
826 ltorque(i) = temp(1,1) + links[i].Im*links[i].Gr*links[i].Gr*qpp(i);
829 ltorque.
Release();
return ltorque;
840 for(i = 1; i <= dof; i++) {
842 if(links[i].get_joint_type() == 0)
848 for(i = dof; i >= 1; i--)
850 F[i] = vp[i]*links[i].m;
859 f[i] = links[i+1].R*f[i+1] + F[i];
860 n[i] = links[i+1].R*n[i+1] +
CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
863 if(links[i].get_joint_type() == 0)
864 temp = (z0.t()*n[i]);
866 temp = (z0.t()*f[i]);
867 ltorque(i) = temp(1,1);
870 ltorque.
Release();
return ltorque;
879 if(qp.
Nrows() != dof) error(
"qp has wrong dimension");
883 for(i = 1; i <= dof; i++) {
885 if(links[i].get_joint_type() == 0)
887 w[i] = Rt*w[i-1] + z0*qp(i);
903 for(i = dof; i >= 1; i--)
905 F[i] = vp[i]*links[i].m +
CrossProduct(wp[i], links[i].mc) +
907 N[i] = links[i].I*wp[i] +
CrossProduct(w[i],links[i].I*w[i]) +
916 f[i] = links[i+1].R*f[i+1] + F[i];
917 n[i] = links[i+1].R*n[i+1] +
CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
920 if(links[i].get_joint_type() == 0)
921 temp = (z0.t()*n[i]);
923 temp = (z0.t()*f[i]);
924 ltorque(i) = temp(1,1)
925 + links[i].Gr*(links[i].Gr*links[i].B*qp(i) + links[i].Cf*
sign(qp(i)));
928 ltorque.
Release();
return ltorque;
virtual ReturnMatrix torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
Joint torque, without contact force, based on Recursive Newton-Euler formulation. ...
Robots class definitions.
virtual ReturnMatrix G()
Joint torque due to gravity based on Recursive Newton-Euler formulation.
static const char rcsid[]
RCS/CVS version.
virtual ReturnMatrix torque_novelocity(const ColumnVector &qpp)
Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation.
virtual ReturnMatrix C(const ColumnVector &qp)
Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.
ReturnMatrix inertia(const ColumnVector &q)
Inertia of the manipulator.
ReturnMatrix acceleration(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &tau)
Joints acceleration without contact force.
virtual ReturnMatrix torque_novelocity(const ColumnVector &qpp)
Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation.
GetSubMatrix Column(int f) const
virtual ReturnMatrix C(const ColumnVector &qp)
Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.
virtual ReturnMatrix torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
Joint torque, without contact force, based on Recursive Newton-Euler formulation. ...
TransposedMatrix t() const
Matrix CrossProduct(const Matrix &A, const Matrix &B)
The usual rectangular matrix.
virtual ReturnMatrix G()
Joint torque due to gravity based on Recursive Newton-Euler formulation.
FloatVector FloatVector * a
virtual ReturnMatrix torque_novelocity(const ColumnVector &qpp)
Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation.
virtual ReturnMatrix C(const ColumnVector &qp)
Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.
virtual ReturnMatrix torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
Joint torque without contact force based on Recursive Newton-Euler formulation.
virtual ReturnMatrix G()
Joint torque due to gravity based on Recursive Newton-Euler formulation.