53 static const char rcsid[] =
"$Id: comp_dqp.cpp,v 1.16 2004/07/06 02:16:36 gourdeau Exp $";
60 using namespace NEWMAT;
75 if(q.
Ncols() != 1 || q.
Nrows() != dof) error(
"q has wrong dimension");
76 if(qp.
Ncols() != 1 || qp.
Nrows() != dof) error(
"qp has wrong dimension");
77 if(dqp.
Ncols() != 1 || qp.
Nrows() != dof) error(
"dqp has wrong dimension");
84 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
90 for(i = 1; i <= dof; i++)
94 p[i](1) = links[i].get_a();
95 p[i](2) = links[i].get_d() * Rt(2,3);
96 p[i](3) = links[i].get_d() * Rt(3,3);
97 if(links[i].get_joint_type() != 0)
104 if(links[i].get_joint_type() == 0)
106 w[i] = Rt*(w[i-1] + z0*qp(i));
107 dw[i] = Rt*(dw[i-1] + z0*dqp(i));
109 dwp[i] = Rt*(dwp[i-1]
125 dwp[i] = Rt*dwp[i-1];
130 dvp[i] = Rt*(dvp[i-1]
146 for(i = dof; i >= 1; i--)
148 F[i] =
a[i] * links[i].m;
149 N[i] = links[i].I*wp[i] +
CrossProduct(w[i],links[i].I*w[i]);
150 dF[i] = da[i] * links[i].m;
151 dN[i] = links[i].I*dwp[i] +
CrossProduct(dw[i],links[i].I*w[i])
164 f[i] = links[i+1].R*f[i+1] + F[i];
165 df[i] = links[i+1].R*df[i+1] + dF[i];
171 if(links[i].get_joint_type() == 0)
173 temp = ((z0.
t()*links[i].R)*n[i]);
174 ltorque(i) = temp(1,1);
175 temp = ((z0.
t()*links[i].R)*dn[i]);
176 dtorque(i) = temp(1,1);
180 temp = ((z0.
t()*links[i].R)*f[i]);
181 ltorque(i) = temp(1,1);
182 temp = ((z0.
t()*links[i].R)*df[i]);
183 dtorque(i) = temp(1,1);
200 if(q.
Ncols() != 1 || q.
Nrows() != dof) error(
"q has wrong dimension");
201 if(qp.
Ncols() != 1 || qp.
Nrows() != dof) error(
"qp has wrong dimension");
202 if(dqp.
Ncols() != 1 || dqp.
Nrows() != dof) error(
"dqp has wrong dimension");
209 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
214 for(i = 1; i <= dof; i++)
218 if(links[i].get_joint_type() != 0)
225 if(links[i].get_joint_type() == 0)
227 w[i] = Rt*w[i-1] + z0*qp(i);
228 dw[i] = Rt*dw[i-1] + z0*dqp(i);
230 dwp[i] = Rt*dwp[i-1] +
CrossProduct(Rt*dw[i-1],z0*qp(i))
244 dwp[i] = Rt*dwp[i-1];
262 for(i = dof; i >= 1; i--) {
263 F[i] =
a[i] * links[i].m;
264 N[i] = links[i].I*wp[i] +
CrossProduct(w[i],links[i].I*w[i]);
265 dF[i] = da[i] * links[i].m;
266 dN[i] = links[i].I*dwp[i] +
CrossProduct(dw[i],links[i].I*w[i])
278 f[i] = links[i+1].R*f[i+1] + F[i];
279 df[i] = links[i+1].R*df[i+1] + dF[i];
280 n[i] = links[i+1].R*n[i+1] +
CrossProduct(p[i+1],links[i+1].R*f[i+1])
282 dn[i] = links[i+1].R*dn[i+1] +
CrossProduct(p[i+1],links[i+1].R*df[i+1])
286 if(links[i].get_joint_type() == 0)
289 ltorque(i) = temp(1,1);
291 dtorque(i) = temp(1,1);
296 ltorque(i) = temp(1,1);
298 dtorque(i) = temp(1,1);
315 if(q.
Ncols() != 1 || q.
Nrows() != dof) error(
"q has wrong dimension");
316 if(qp.
Ncols() != 1 || qp.
Nrows() != dof) error(
"qp has wrong dimension");
317 if(dqp.
Ncols() != 1 || dqp.
Nrows() != dof) error(
"dqp has wrong dimension");
324 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
329 for(i = 1; i <= dof; i++)
333 if(links[i].get_joint_type() != 0)
340 if(links[i].get_joint_type() == 0)
342 w[i] = Rt*w[i-1] + z0*qp(i);
343 dw[i] = Rt*dw[i-1] + z0*dqp(i);
345 dwp[i] = Rt*dwp[i-1] +
CrossProduct(Rt*dw[i-1],z0*qp(i));
358 dwp[i] = Rt*dwp[i-1];
369 for(i = dof; i >= 1; i--) {
370 F[i] = vp[i]*links[i].m +
CrossProduct(wp[i], links[i].mc) +
372 dF[i] = dvp[i]*links[i].m +
CrossProduct(dwp[i],links[i].mc)
375 N[i] = links[i].I*wp[i] +
CrossProduct(w[i],links[i].I*w[i])
377 dN[i] = links[i].I*dwp[i] +
CrossProduct(dw[i],links[i].I*w[i])
390 f[i] = links[i+1].R*f[i+1] + F[i];
391 df[i] = links[i+1].R*df[i+1] + dF[i];
392 n[i] = links[i+1].R*n[i+1] +
CrossProduct(p[i+1],links[i+1].R*f[i+1]) + N[i];
393 dn[i] = links[i+1].R*dn[i+1] +
CrossProduct(p[i+1],links[i+1].R*df[i+1]) + dN[i];
396 if(links[i].get_joint_type() == 0)
399 ltorque(i) = temp(1,1);
401 dtorque(i) = temp(1,1);
406 ltorque(i) = temp(1,1);
408 dtorque(i) = temp(1,1);
virtual void dqp_torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &dqp, ColumnVector &torque, ColumnVector &dtorque)
Delta torque due to delta joint velocity.
Robots class definitions.
virtual void dqp_torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &dqp, ColumnVector &torque, ColumnVector &dtorque)
Delta torque due to delta joint velocity.
virtual void dqp_torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &dqp, ColumnVector &torque, ColumnVector &dtorque)
Delta torque due to delta joint velocity.
TransposedMatrix t() const
Matrix CrossProduct(const Matrix &A, const Matrix &B)
static const char rcsid[]
RCS/CVS version.
The usual rectangular matrix.
FloatVector FloatVector * a