public void updateJointPositions_ID_to_SCS() { if (scsFloatingJoint != null) { idFloatingJoint.getFrameAfterJoint().getTransformToDesiredFrame(transformToWorld, ReferenceFrame.getWorldFrame()); scsFloatingJoint.setRotationAndTranslation(transformToWorld); } for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints) { OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint); scsJoint.setQ(idJoint.getQ()); } }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { nonZUpFrame.getTransformToDesiredFrame(transformToParent, rootFrame); // Compute the yaw rotation matrix while avoiding the computation of the actual yaw-pitch-roll angles. double sinPitch = -transformToParent.getM20(); cosPitch = Math.sqrt(1.0 - sinPitch * sinPitch); cosRoll = transformToParent.getM22() / cosPitch; sinRoll = transformToParent.getM21() / cosPitch; double cosYaw = transformToParent.getM00() / cosPitch; double sinYaw = transformToParent.getM10() / cosPitch; transformToParent.setRotation(cosYaw, -sinYaw, 0.0, sinYaw, cosYaw, 0.0, 0.0, 0.0, 1.0); }
private void calculateErrorTransform(RigidBodyTransform transformShoulderToDesired) { jacobian.getEndEffector().getBodyFixedFrame().getTransformToDesiredFrame(transformShoulderToEndEffector, jacobian.getBaseFrame()); transformEndEffectorToShoulder.setAndInvert(transformShoulderToEndEffector); transformEndEffectorToDesired.set(transformEndEffectorToShoulder); transformEndEffectorToDesired.multiply(transformShoulderToDesired); }
private void printLinkInformation(RigidBodyBasics link, StringBuffer buffer) { SpatialInertiaBasics inertia = link.getInertia(); JointBasics parentJoint = link.getParentJoint(); if (inertia != null) { double mass = inertia.getMass(); Vector3D comOffset = new Vector3D(); RigidBodyTransform comOffsetTransform = link.getBodyFixedFrame().getTransformToDesiredFrame(parentJoint.getFrameAfterJoint()); comOffsetTransform.getTranslation(comOffset); Matrix3DBasics momentOfInertia = inertia.getMomentOfInertia(); buffer.append("Mass = " + mass + "\n"); buffer.append("comOffset = " + comOffset + "\n"); buffer.append("momentOfInertia = \n" + momentOfInertia + "\n"); } List<? extends JointBasics> childrenJoints = link.getChildrenJoints(); for (JointBasics childJoint : childrenJoints) { String parentJointName; if (parentJoint != null) parentJointName = parentJoint.getName(); else parentJointName = "root joint"; buffer.append("Found Child Joint of " + parentJointName + ".\n"); printJointInformation(childJoint, buffer); } }
private Point3D getMidFootPoint() { RigidBodyTransform temp = new RigidBodyTransform(); Point3D left = new Point3D(); Point3D avg = new Point3D(); fullRobotModel.getFoot(RobotSide.LEFT).getBodyFixedFrame().getTransformToDesiredFrame(temp, ReferenceFrame.getWorldFrame()); temp.transform(left); fullRobotModel.getFoot(RobotSide.RIGHT).getBodyFixedFrame().getTransformToDesiredFrame(temp, ReferenceFrame.getWorldFrame()); temp.transform(avg); avg.add(left); avg.scale(0.5); return avg; }
/** * The first pass consists in calculating the bias wrench resulting from external and Coriolis * forces, and the bias acceleration resulting from the Coriolis acceleration. */ public void passOne() { if (!isRoot()) { MovingReferenceFrame frameAfterJoint = getFrameAfterJoint(); MovingReferenceFrame frameBeforeJoint = getJoint().getFrameBeforeJoint(); if (parent.isRoot()) frameAfterJoint.getTransformToDesiredFrame(transformToParentJointFrame, parent.getBodyFixedFrame()); else frameAfterJoint.getTransformToDesiredFrame(transformToParentJointFrame, parent.getFrameAfterJoint()); bodyInertia.computeDynamicWrench(null, getBodyTwist(), biasWrench); biasWrench.sub(externalWrench); biasWrench.changeFrame(frameAfterJoint); biasAcceleration.setToZero(frameAfterJoint, input.getInertialFrame(), frameBeforeJoint); biasAcceleration.changeFrame(frameAfterJoint, getJoint().getJointTwist(), frameAfterJoint.getTwistOfFrame()); biasAcceleration.get(c); } for (int childIndex = 0; childIndex < children.size(); childIndex++) children.get(childIndex).passOne(); }
handControlFrame.getTransformToDesiredFrame(handControlFrameTransformToBodyFixedFrame, hand.getBodyFixedFrame()); trajectory.getControlFramePositionInEndEffector().set(handControlFrameTransformToBodyFixedFrame.getTranslationVector()); trajectory.getControlFrameOrientationInEndEffector().set(handControlFrameTransformToBodyFixedFrame.getRotationMatrix());