// Override featherstonePassOne since don't need to do everything...
public void featherstonePassOne(Vector3d w_h, Vector3d v_h, Matrix3d Rh_0) {
owner.update();
owner.jointTransform3D.get(Ri_0);
// this.jointDependentSetAndGetRotation(Ri_0);
Ri_0.transpose();
Rh_i = null;
Ri_h = null;
d_i = null;
u_iXd_i = null;
r_i = null;
// r_h = null;
// Now do the joint dependent stuff...
this.jointDependentFeatherstonePassOne();
// Now update the points attached to the joint:
R0_i.set(Ri_0);
R0_i.transpose();
// +++JEP OPTIMIZE
if (groundContactPointGroups != null) {
Collection<GroundContactPointGroup> groups = groundContactPointGroups.values();
for (GroundContactPointGroup groundContactPointGroup : groups) {
ArrayList<GroundContactPoint> groundContactPoints =
groundContactPointGroup.getGroundContactPointsInContact();
for (int i = 0; i < groundContactPoints.size(); i++) {
GroundContactPoint point = groundContactPoints.get(i);
point.updatePointVelocity(R0_i, owner.link.comOffset, v_i, w_i);
}
}
}
if (kinematicPoints != null) {
for (int i = 0; i < kinematicPoints.size(); i++) {
KinematicPoint point = kinematicPoints.get(i);
point.updatePointVelocity(R0_i, owner.link.comOffset, v_i, w_i);
}
}
// Recurse over the children:
for (int i = 0; i < owner.childrenJoints.size(); i++) {
Joint child = (Joint) owner.childrenJoints.get(i);
child.physics.featherstonePassOne(w_i, v_i, Ri_0);
}
}
protected void propagateImpulseSetDeltaVOnPath(
SpatialVector delta_v_parent, SpatialVector delta_v_me) {
// Override with FloatingJoint as in Mirtich p. 144
// Already have computed I_hat_inverse from the dynamics. Use that...
Y_hat_i.getMatrix(Y_hat_matrix);
// a_hat_matrix = I_hat_inverse.times(Y_hat_matrix);
a_hat_matrix.timesEquals(I_hat_inverse, Y_hat_matrix);
// System.out.println("Y_hat_i: " + Y_hat_i);
delta_v_me.top.set(-a_hat_matrix.get(0, 0), -a_hat_matrix.get(1, 0), -a_hat_matrix.get(2, 0));
delta_v_me.bottom.set(
-a_hat_matrix.get(3, 0), -a_hat_matrix.get(4, 0), -a_hat_matrix.get(5, 0));
// These are defined in body coords. Don't rotate.
owner.qd_wx.set(owner.qd_wx.getDoubleValue() + delta_v_me.top.x);
owner.qd_wy.set(owner.qd_wy.getDoubleValue() + delta_v_me.top.y);
owner.qd_wz.set(owner.qd_wz.getDoubleValue() + delta_v_me.top.z);
// Rotate into world coords.
delta_qd_xyz.set(delta_v_me.bottom);
R0_i.transform(delta_qd_xyz);
owner.qd_x.set(owner.qd_x.getDoubleValue() + delta_qd_xyz.x);
owner.qd_y.set(owner.qd_y.getDoubleValue() + delta_qd_xyz.y);
owner.qd_z.set(owner.qd_z.getDoubleValue() + delta_qd_xyz.z);
}
public void featherstonePassFour(SpatialVector a_hat_h, int passNumber)
throws UnreasonableAccelerationException {
I_hat_i.getMatrix(I_hat_matrix);
// if (I_hat_inverse == null) I_hat_inverse = new Matrix(I_hat_matrix.getRowDimension(),
// I_hat_matrix.getColumnDimension());
I_hat_matrix.inverse(I_hat_inverse);
Z_hat_i.getMatrix(Z_hat_matrix);
// System.out.println(Z_hat_i);
// a_hat_matrix = I_hat_inverse.times(Z_hat_matrix);
a_hat_matrix.timesEquals(I_hat_inverse, Z_hat_matrix);
a_hat_i.top.set(-a_hat_matrix.get(0, 0), -a_hat_matrix.get(1, 0), -a_hat_matrix.get(2, 0));
a_hat_i.bottom.set(-a_hat_matrix.get(3, 0), -a_hat_matrix.get(4, 0), -a_hat_matrix.get(5, 0));
// +++JEP 10/17/01. Transform to joint location from com location.
// +++TK 02/06/12 THIS WAS WRONG FOR 11 YEARS! We need a_hat_i to be expressed in com frame for
// the rest of the dynamics algorithm.
// Transformation to joint location should be done as post processing only (to set the qdd_*
// variables)
// Now doing this same transformation on data that does not matter for the rest of the algorithm
// wXr.cross(w_i, this.link.comOffset);
// wXwXr.cross(w_i, wXr);
// wdXr.cross(a_hat_i.top, this.link.comOffset);
// a_hat_i.bottom.sub(wdXr);
// a_hat_i.bottom.sub(wXwXr);
// //////////////////
// Rotate into world coords...
R0_i.set(Ri_0);
R0_i.transpose();
a_hat_world_top.set(a_hat_i.top);
a_hat_world_bot.set(a_hat_i.bottom);
// //////////////////
// +++JEP 10/17/01. Transform to joint location from com location.
// +++TK 02/06/12 See comment above.
wXr.cross(w_i, owner.link.comOffset);
wXwXr.cross(w_i, wXr);
wdXr.cross(a_hat_world_top, owner.link.comOffset);
a_hat_world_bot.sub(wdXr);
a_hat_world_bot.sub(wXwXr);
// R0_i.transform(a_hat_world_top); //+++JEP w and wd should be in joint coords, not world
// coords. Only x,y,z in world coords.
R0_i.transform(a_hat_world_bot);
owner.qdd_x.set(a_hat_world_bot.x);
owner.qdd_y.set(a_hat_world_bot.y);
owner.qdd_z.set(a_hat_world_bot.z);
owner.qdd_wx.set(a_hat_world_top.x);
owner.qdd_wy.set(a_hat_world_top.y);
owner.qdd_wz.set(a_hat_world_top.z);
jointDependentRecordK(passNumber);
/*
* k_qdd_x[passNumber] = qdd_x.getDoubleValue(); k_qdd_y[passNumber] = qdd_y.getDoubleValue(); k_qdd_z[passNumber] = qdd_z.getDoubleValue();
* k_qd_x[passNumber] = qd_x.getDoubleValue(); k_qd_y[passNumber] = qd_y.getDoubleValue(); k_qd_z[passNumber] = qd_z.getDoubleValue();
* k_qdd_wx[passNumber] = qdd_wx.getDoubleValue(); k_qdd_wy[passNumber] = qdd_wy.getDoubleValue(); k_qdd_wz[passNumber] = qdd_wz.getDoubleValue();
* k_qd_wx[passNumber] = qd_wx.getDoubleValue(); k_qd_wy[passNumber] = qd_wy.getDoubleValue(); k_qd_wz[passNumber] = qd_wz.getDoubleValue();
*
* k_qd_qs[passNumber] = 0.5*(-q_qx.getDoubleValue() * qd_wx.getDoubleValue() - q_qy.getDoubleValue() * qd_wy.getDoubleValue() - q_qz.getDoubleValue() * qd_wz.getDoubleValue());
* k_qd_qx[passNumber] = 0.5*(+q_qs.getDoubleValue() * qd_wx.getDoubleValue() - q_qz.getDoubleValue() * qd_wy.getDoubleValue() + q_qy.getDoubleValue() * qd_wz.getDoubleValue());
* k_qd_qy[passNumber] = 0.5*(+q_qz.getDoubleValue() * qd_wx.getDoubleValue() + q_qs.getDoubleValue() * qd_wy.getDoubleValue() - q_qx.getDoubleValue() * qd_wz.getDoubleValue());
* k_qd_qz[passNumber] = 0.5*(-q_qy.getDoubleValue() * qd_wx.getDoubleValue() + q_qx.getDoubleValue() * qd_wy.getDoubleValue() + q_qs.getDoubleValue() * qd_wz.getDoubleValue());
*/
// Check for unreasonable accelerations:
if (!jointDependentVerifyReasonableAccelerations()) {
ArrayList<Joint> unreasonableAccelerationJoints = new ArrayList<Joint>();
unreasonableAccelerationJoints.add(owner);
throw new UnreasonableAccelerationException(unreasonableAccelerationJoints);
}
for (int i = 0; i < owner.childrenJoints.size(); i++) {
Joint child = (Joint) owner.childrenJoints.get(i);
child.physics.featherstonePassFour(a_hat_i, passNumber);
}
// System.out.println(this);
}
