/** * Computes and returns the joint torque vector that corresponds to the given wrench. * * @param wrench the resulting wrench at the end effector. * The wrench should be expressed in {@code jacobianFrame} and the wrench's {@code bodyFrame} * should be the body fixed frame of the end-effector. * @return joint torques that result in the given wrench as a column vector. * @throws ReferenceFrameMismatchException if the given wrench * {@code wrench.getExpressedInFrame() != this.getJacobianFrame()} or * {@code wrench.getBodyFrame() != this.getEndEffectorFrame()}. */ public DenseMatrix64F computeJointTorques(Wrench wrench) { DenseMatrix64F jointTorques = new DenseMatrix64F(1, jacobian.getNumCols()); computeJointTorques(wrench, jointTorques); return jointTorques; }
/** * Computes and returns the joint torque vector that corresponds to the given wrench. * * @param wrench the resulting wrench at the end effector. The wrench should be expressed in * {@code jacobianFrame} and the wrench's {@code bodyFrame} should be the body fixed * frame of the end-effector. * @return joint torques that result in the given wrench as a column vector. * @throws ReferenceFrameMismatchException if the given wrench * {@code wrench.getExpressedInFrame() != this.getJacobianFrame()} or * {@code wrench.getBodyFrame() != this.getEndEffectorFrame()}. */ public DenseMatrix64F computeJointTorques(WrenchReadOnly wrench) { DenseMatrix64F jointTorques = new DenseMatrix64F(1, jacobian.getNumCols()); computeJointTorques(wrench, jointTorques); return jointTorques; }
public void update() { for (int i = 0; i < allRigidBodies.size(); i++) { RigidBody rigidBody = allRigidBodies.get(i); FootSwitchInterface footSwitch = footSwitches.get(rigidBody); GeometricJacobian jacobian = jacobians.get(rigidBody); footSwitch.computeAndPackFootWrench(wrench); wrench.changeFrame(rigidBody.getBodyFixedFrame()); wrench.negate(); jacobian.compute(); jacobian.computeJointTorques(wrench, jointTorquesMatrix); InverseDynamicsJoint[] joints = jacobian.getJointsInOrder(); for (int j = 0; j < joints.length; j++) { OneDoFJoint joint = (OneDoFJoint) joints[j]; jointTorques.get(joint).set(jointTorquesMatrix.get(j, 0)); } } } }