/** * Returns the body fixed frame of the base {@code RigidBody} of this Jacobian. * The base is the predecessor of the first joint that this Jacobian considers. * * @return the body fixed frame of the base. */ public ReferenceFrame getBaseFrame() { return getBase().getBodyFixedFrame(); }
@Override public void setSuccessor(RigidBody successor) { this.successor = successor; setMotionSubspace(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); this.successorWrench = new Wrench(successorFrame, successorFrame); }
@Override public void getSuccessorAcceleration(SpatialAccelerationVector accelerationToPack) { getJointAcceleration(accelerationToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); accelerationToPack.changeBaseFrameNoRelativeAcceleration(predecessorFrame); accelerationToPack.changeBodyFrameNoRelativeAcceleration(successorFrame); accelerationToPack.changeFrameNoRelativeMotion(successorFrame); }
public void setBodyFixedPointToControl(FramePoint bodyFixedPointInEndEffectorFrame) { bodyFixedPointInEndEffectorFrame.checkReferenceFrameMatch(endEffector.getBodyFixedFrame()); bodyFixedPointToControl.setIncludingFrame(bodyFixedPointInEndEffectorFrame); }
@Override public void getSuccessorTwist(Twist twistToPack) { getJointTwist(twistToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); twistToPack.changeBaseFrameNoRelativeTwist(predecessorFrame); twistToPack.changeBodyFrameNoRelativeTwist(successorFrame); twistToPack.changeFrame(successorFrame); }
@Override public void getDesiredSuccessorAcceleration(SpatialAccelerationVector accelerationToPack) { getDesiredJointAcceleration(accelerationToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); accelerationToPack.changeBaseFrameNoRelativeAcceleration(predecessorFrame); accelerationToPack.changeBodyFrameNoRelativeAcceleration(successorFrame); accelerationToPack.changeFrameNoRelativeMotion(successorFrame); }
@Override public void setSuccessor(RigidBody successor) { this.successor = successor; setMotionSubspace(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); this.successorWrench = new Wrench(successorFrame, successorFrame); }
@Override public void getPredecessorTwist(Twist twistToPack) { getJointTwist(twistToPack); ReferenceFrame predecessorFrame = predecessor.getBodyFixedFrame(); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); twistToPack.changeBaseFrameNoRelativeTwist(predecessorFrame); twistToPack.changeBodyFrameNoRelativeTwist(successorFrame); twistToPack.invert(); twistToPack.changeFrame(predecessorFrame); }
public void changeFrameAndSetControlFrameFixedInEndEffector(FramePoint position, FrameOrientation orientation) { RigidBody endEffector = spatialAccelerationCommand.getEndEffector(); position.changeFrame(endEffector.getBodyFixedFrame()); orientation.changeFrame(endEffector.getBodyFixedFrame()); position.get(controlFrameOriginInEndEffectorFrame); orientation.getQuaternion(controlFrameOrientationInEndEffectorFrame); }
public void changeFrameAndSetControlFrameFixedInEndEffector(FramePose pose) { RigidBody endEffector = spatialAccelerationCommand.getEndEffector(); pose.changeFrame(endEffector.getBodyFixedFrame()); pose.getPose(controlFrameOriginInEndEffectorFrame, controlFrameOrientationInEndEffectorFrame); }
public void set(RigidBody rigidBody, Wrench externalWrench) { setRigidBody(rigidBody); externalWrenchAppliedOnRigidBody.set(externalWrench); externalWrenchAppliedOnRigidBody.changeFrame(rigidBody.getBodyFixedFrame()); }
@Override public void variableChanged(YoVariable<?> v) { rigidBodyCoMOffset.getFrameTupleIncludingFrame(tempFramePoint); tempFramePoint.changeFrame(rigidBody.getBodyFixedFrame()); rigidBody.setCoMOffset(tempFramePoint); } };
public void set(RigidBody controlledBody, Wrench virtualWrench) { setRigidBody(controlledBody); virtualWrenchAppliedByRigidBody.set(virtualWrench); virtualWrenchAppliedByRigidBody.changeFrame(controlledBody.getBodyFixedFrame()); }
private void calculateErrorTransform(RigidBodyTransform transformShoulderToDesired) { jacobian.getEndEffector().getBodyFixedFrame().getTransformToDesiredFrame(transformShoulderToEndEffector, jacobian.getBaseFrame()); transformEndEffectorToShoulder.invert(transformShoulderToEndEffector); transformEndEffectorToDesired.multiply(transformEndEffectorToShoulder, transformShoulderToDesired); }
private double getAngleToPelvis(Point3d point, Point3d lidarOrigin) { RigidBodyTransform tf = new RigidBodyTransform(); ReferenceFrame.getWorldFrame().getTransformToDesiredFrame(tf, fullRobotModel.getPelvis().getBodyFixedFrame()); Point3d tfPoint = new Point3d(point); tf.transform(tfPoint); return Math.atan2(tfPoint.getY(), tfPoint.getX()); }
public static PassiveRevoluteJoint addPassiveRevoluteJoint(String jointName, RigidBody parentBody, RigidBodyTransform transformToParent, Vector3d jointAxis, boolean isPartOfClosedKinematicLoop) { String beforeJointName = "before" + jointName; ReferenceFrame parentFrame; if (parentBody.isRootBody()) parentFrame = parentBody.getBodyFixedFrame(); else parentFrame = parentBody.getParentJoint().getFrameAfterJoint(); ReferenceFrame frameBeforeJoint = createOffsetFrame(parentFrame, transformToParent, beforeJointName); String afterJointName = jointName; return new PassiveRevoluteJoint(afterJointName, parentBody, frameBeforeJoint, new FrameVector(frameBeforeJoint, jointAxis), isPartOfClosedKinematicLoop); }
private boolean isAheadOfPelvis(Point3d point) { RigidBodyTransform tf = new RigidBodyTransform(); ReferenceFrame.getWorldFrame().getTransformToDesiredFrame(tf, fullRobotModel.getPelvis().getBodyFixedFrame()); Point3d tfPoint = new Point3d(point); tf.transform(tfPoint); return tfPoint.getX() > parameters.xCutoffPelvis; }
@Override public void setTorqueFromWrench(Wrench jointWrench) { jointWrench.getBodyFrame().checkReferenceFrameMatch(successor.getBodyFixedFrame()); jointWrench.getExpressedInFrame().checkReferenceFrameMatch(jointTorque.getReferenceFrame()); jointTorque.set(jointWrench.getAngularPart()); }
private void sequenceHandShakePrep() { RobotSide robotSide = RobotSide.RIGHT; boolean mirrorOrientationForRightSide = true; FrameOrientation desiredUpperArmOrientation = new FrameOrientation(fullRobotModel.getChest().getBodyFixedFrame()); FrameOrientation desiredHandOrientation = new FrameOrientation(lowerArmsFrames.get(robotSide)); desiredUpperArmOrientation.setYawPitchRoll(0.0, -shoulderExtensionAngle, -1.2708); desiredHandOrientation.setYawPitchRoll(0.0, Math.PI / 2.0, 0.0); submitHandPose(robotSide, desiredUpperArmOrientation, shoulderExtensionAngle - Math.PI / 2.0, desiredHandOrientation, mirrorOrientationForRightSide); pipeLine.requestNewStage(); }
public void computeChestTrajectoryMessage() { checkIfDataHasBeenSet(); ReferenceFrame chestFrame = fullRobotModelToUseForConversion.getChest().getBodyFixedFrame(); Quat4d desiredQuaternion = new Quat4d(); FrameOrientation desiredOrientation = new FrameOrientation(chestFrame); desiredOrientation.changeFrame(worldFrame); desiredOrientation.getQuaternion(desiredQuaternion); ChestTrajectoryMessage chestTrajectoryMessage = new ChestTrajectoryMessage(trajectoryTime, desiredQuaternion); output.setChestTrajectoryMessage(chestTrajectoryMessage); }