public void get(Point3d positionToPack) { currentPosition.get(positionToPack); }
@Override public void getPoint(Point3d pointToPack) { this.get(pointToPack); }
@Override public Point3d getMeasurementPointInBodyFrame() { yoMeasurementPointInBodyFrame.get(measurementPointInBodyFrame); return measurementPointInBodyFrame; }
@Override public void getPosition(Point3d positionToPack) { position.get(positionToPack); }
@Override public void getPosition(Point3d positionToPack) { position.get(positionToPack); }
@Override public void getPosition(Point3d positionToPack) { position.get(positionToPack); }
@Override public void getPosition(Point3d positionToPack) { position.get(positionToPack); }
public void get(Point3d positionToPack) { currentPosition.get(positionToPack); }
@Override public Point3d getMeasurementPointInBodyFrame() { yoMeasurementPointInBodyFrame.get(measurementPointInBodyFrame); return super.getMeasurementPointInBodyFrame(); }
public void get(Vector3d translation) { yoFramePose.getPosition().get(translation); } }
@Override public Point3d getMeasurementPointInWorldFrame() { yoMeasurementPointInWorldFrame.get(positionOfMeasurementPointInWorldFrame); return super.getMeasurementPointInWorldFrame(); }
@Override public void initialize() { robot.update(); for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { externalForcePoints.get(i).getYoPosition().get(initialPositions.get(i)); desiredHeight.add(initialPositions.get(i).getZ() / initialPositions.size()); efp_positionViz.get(i).update(); } for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); doControl(); }
@Override public void initialize() { robot.update(); for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { externalForcePoints.get(i).getYoPosition().get(initialPositions.get(i)); desiredHeight.add(initialPositions.get(i).getZ() / initialPositions.size()); efp_positionViz.get(i).update(); } for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); doControl(); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { circleOrigin.get(localTranslation); rotationAxis.get(localRotationAxis); GeometryTools.getAxisAngleFromZUpToVector(localRotationAxis, localAxisAngle); transformToParent.set(localAxisAngle, localTranslation); } };
@Override protected void putYoValuesIntoFrameWaypoint() { EuclideanWaypoint simpleWaypoint = frameWaypoint.getGeometryObject(); position.get(simpleWaypoint.getPosition()); linearVelocity.get(simpleWaypoint.getLinearVelocity()); }
@Override public void doControl() { for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); ExternalForcePoint efp = externalForcePoints.get(i); efp.getYoPosition().get(proportionalTerm); proportionalTerm.sub(initialPositions.get(i)); proportionalTerm.scale(-holdPelvisKp.getDoubleValue()); // proportionalTerm.setZ(Math.max(proportionalTerm.getZ(), 0.0)); efp.getYoVelocity().get(derivativeTerm); derivativeTerm.scale(-holdPelvisKv.getDoubleValue()); pdControlOutput.add(proportionalTerm, derivativeTerm); efp.setForce(pdControlOutput); efp.getYoForce().getYoZ().add(robotWeight / efp_offsetFromRootJoint.size()); efp_positionViz.get(i).update(); } }
public void updateForFrozenState() { // Keep setting the position so the localization updater works properly. yoRootJointPosition.get(tempRootJointTranslation); rootJoint.setPosition(tempRootJointTranslation); // Set the rootJoint twist to zero. rootJoint.getJointTwist(rootJointTwist); rootJointTwist.setToZero(); rootJoint.setJointTwist(rootJointTwist); rootJointFrame.update(); twistCalculator.compute(); updateCoMState(); }
private void updateRootJoint() { yoRootJointPosition.get(tempRootJointTranslation); rootJoint.setPosition(tempRootJointTranslation); tempVelocity.setIncludingFrame(rootJointVelocity); rootJoint.getJointTwist(rootJointTwist); tempVelocity.changeFrame(rootJointTwist.getExpressedInFrame()); rootJointTwist.setLinearPart(tempVelocity); rootJoint.setJointTwist(rootJointTwist); rootJointFrame.update(); twistCalculator.compute(); }
@Override protected void putYoValuesIntoFrameWaypoint() { SE3Waypoint simpleWaypoint = frameWaypoint.getGeometryObject(); EuclideanWaypoint euclideanWaypoint = simpleWaypoint.getEuclideanWaypoint(); SO3Waypoint so3Waypoint = simpleWaypoint.getSO3Waypoint(); position.get(euclideanWaypoint.getPosition()); orientation.get(so3Waypoint.getOrientation()); linearVelocity.get(euclideanWaypoint.getLinearVelocity()); angularVelocity.get(so3Waypoint.getAngularVelocity()); }
@Override protected void updateTransformToParent(RigidBodyTransform transformToParent) { steeringWheelRotationAxis.get(localZAxis); steeringWheelZeroAxis.get(localXAxis); localYAxis.cross(localZAxis, localXAxis); localYAxis.normalize(); localXAxis.cross(localYAxis, localZAxis); steeringWheelZeroAxis.set(localXAxis); steeringWheelCenter.get(localTranslation); localRotation.setColumn(0, localXAxis); localRotation.setColumn(1, localYAxis); localRotation.setColumn(2, localZAxis); transformToParent.set(localRotation, localTranslation); } };