public VirtualChainDataPoint(
ReferenceFrame baseFrame,
ArrayList<ReferenceFrame> referenceFrames,
FramePoint2d centerOfMassProjection)
// public VirtualChainDataPoint(ReferenceFrame baseFrame, ArrayList<ReferenceFrame>
// referenceFrames, FramePoint centerOfMassProjection)
{
centerOfMassProjection.checkReferenceFrameMatch(baseFrame);
if (!centerOfMassProjection.getReferenceFrame().isZupFrame()) {
throw new RuntimeException("centerOfMassProjection is Not a ZUp reference frame!");
}
this.centerOfMassProjection = new FramePoint2d(centerOfMassProjection);
// centerOfMassProjection.changeFrame(baseFrame);
rotationMatrices = new ArrayList<Matrix3d>();
for (int i = 0; i < referenceFrames.size(); i++) {
ReferenceFrame virtualChainFrame = referenceFrames.get(i);
RigidBodyTransform transform3D = virtualChainFrame.getTransformToDesiredFrame(baseFrame);
Matrix3d rotationMatrix = new Matrix3d();
transform3D.get(rotationMatrix);
rotationMatrices.add(rotationMatrix);
}
}
private Matrix3d computeDesiredFootRotation(
double angleToDestination, RobotSide swingLegSide, ReferenceFrame supportFootFrame) {
RigidBodyTransform supportFootToWorldTransform =
supportFootFrame.getTransformToDesiredFrame(ReferenceFrame.getWorldFrame());
Matrix3d supportFootToWorldRotation = new Matrix3d();
supportFootToWorldTransform.get(supportFootToWorldRotation);
double maxTurnInAngle = 0.25;
double maxTurnOutAngle = 0.4;
double amountToYaw;
switch (blindWalkingDirection.getEnumValue()) {
case BACKWARD:
{
amountToYaw = AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI);
break;
}
case LEFT:
{
amountToYaw =
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, Math.PI / 2.0);
break;
}
case RIGHT:
{
amountToYaw =
AngleTools.computeAngleDifferenceMinusPiToPi(angleToDestination, -Math.PI / 2.0);
break;
}
case FORWARD:
{
amountToYaw = angleToDestination;
break;
}
default:
{
throw new RuntimeException("Shouldn't get here!");
}
}
if (swingLegSide == RobotSide.LEFT) {
amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnInAngle, maxTurnOutAngle);
} else {
amountToYaw = MathTools.clipToMinMax(amountToYaw, -maxTurnOutAngle, maxTurnInAngle);
}
Matrix3d yawRotation = new Matrix3d();
yawRotation.rotZ(amountToYaw);
Matrix3d ret = new Matrix3d();
ret.mul(yawRotation, supportFootToWorldRotation);
return ret;
}
@ContinuousIntegrationTest(estimatedDuration = 0.2)
@Test(timeout = 30000)
public void testChangeFrame() throws Exception {
double epsilon = 1.0e-10;
Random random = new Random(21651016L);
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
ReferenceFrame expectedFrame = worldFrame;
double expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
FrameOrientation expectedOrientation =
FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
FrameVector expectedAngularVelocity =
FrameVector.generateRandomFrameVector(random, expectedFrame);
String expectedNamePrefix = "test";
String expectedNameSuffix = "blop";
YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(
expectedNamePrefix,
expectedNameSuffix,
new YoVariableRegistry("schnoop"),
expectedFrame);
testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);
ReferenceFrame[] randomFrames = new ReferenceFrame[10];
randomFrames[0] = worldFrame;
for (int i = 1; i < 10; i++)
randomFrames[i] =
ReferenceFrame.generateRandomReferenceFrame(
"randomFrame" + i,
random,
random.nextBoolean() ? worldFrame : randomFrames[random.nextInt(i)]);
for (int i = 0; i < 10000; i++) {
expectedFrame = randomFrames[random.nextInt(10)];
testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(expectedFrame);
expectedOrientation.changeFrame(expectedFrame);
expectedAngularVelocity.changeFrame(expectedFrame);
testedYoFrameSO3TrajectoryPoint.changeFrame(expectedFrame);
assertWaypointContainsExpectedData(
expectedNamePrefix,
expectedNameSuffix,
expectedFrame,
expectedTime,
expectedOrientation,
expectedAngularVelocity,
testedYoFrameSO3TrajectoryPoint,
epsilon);
}
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testCommonUsageExample() {
String namePrefix = "point";
String nameSuffix = "toTest";
YoVariableRegistry registry = new YoVariableRegistry("myRegistry");
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
PoseReferenceFrame poseFrame = new PoseReferenceFrame("poseFrame", new FramePose(worldFrame));
FramePoint poseFramePosition = new FramePoint(worldFrame, new Point3d(0.5, 7.7, 9.2));
poseFrame.setPositionAndUpdate(poseFramePosition);
FrameOrientation poseOrientation =
new FrameOrientation(worldFrame, new AxisAngle4d(1.2, 3.9, 4.7, 2.2));
poseFrame.setOrientationAndUpdate(poseOrientation);
YoFrameSO3TrajectoryPoint yoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, worldFrame, poseFrame);
SimpleSO3TrajectoryPoint simpleTrajectoryPoint = new SimpleSO3TrajectoryPoint();
double time = 3.4;
TransformableQuat4d orientation = new TransformableQuat4d(new Quat4d(0.1, 0.22, 0.34, 0.56));
orientation.normalize();
Vector3d angularVelocity = new Vector3d(1.7, 8.4, 2.2);
simpleTrajectoryPoint.set(time, orientation, angularVelocity);
yoFrameSO3TrajectoryPoint.setIncludingFrame(worldFrame, simpleTrajectoryPoint);
yoFrameSO3TrajectoryPoint.changeFrame(poseFrame);
// Do some checks:
RigidBodyTransform transformToPoseFrame = worldFrame.getTransformToDesiredFrame(poseFrame);
orientation.applyTransform(transformToPoseFrame);
transformToPoseFrame.transform(angularVelocity);
namePrefix = "point";
nameSuffix = "toVerify";
YoFrameSO3TrajectoryPoint expectedYoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(namePrefix, nameSuffix, registry, poseFrame);
expectedYoFrameSO3TrajectoryPoint.setTime(time);
expectedYoFrameSO3TrajectoryPoint.setOrientation(orientation);
expectedYoFrameSO3TrajectoryPoint.setAngularVelocity(angularVelocity);
assertEquals(3.4, yoFrameSO3TrajectoryPoint.getTime(), 1e-7);
assertEquals(3.4, expectedYoFrameSO3TrajectoryPoint.getTime(), 1e-7);
assertTrue(expectedYoFrameSO3TrajectoryPoint.epsilonEquals(yoFrameSO3TrajectoryPoint, 1e-10));
}
public GroundContactPoint(String name, Vector3d offset, YoVariableRegistry registry) {
super(name, offset, registry);
touchdownLocation =
new YoFramePoint(name + "_td", "", ReferenceFrame.getWorldFrame(), registry);
fs = new DoubleYoVariable(name + "_fs", "GroundContactPoint foot switch", registry);
slip = new BooleanYoVariable(name + "_slip", "GroundContactPoint slipping", registry);
collisionCount =
new IntegerYoVariable(name + "_coll", "GroundContactPoint colliding", registry);
surfaceNormal = new YoFrameVector(name + "_n", "", ReferenceFrame.getWorldFrame(), registry);
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testConstructor() {
double epsilon = 1.0e-20;
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
ReferenceFrame expectedFrame = worldFrame;
double expectedTime = 0.0;
FrameOrientation expectedOrientation = new FrameOrientation(expectedFrame);
FrameVector expectedAngularVelocity = new FrameVector(expectedFrame);
String expectedNamePrefix = "test";
String expectedNameSuffix = "blop";
YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(
expectedNamePrefix,
expectedNameSuffix,
new YoVariableRegistry("schnoop"),
expectedFrame);
assertWaypointContainsExpectedData(
expectedNamePrefix,
expectedNameSuffix,
expectedFrame,
expectedTime,
expectedOrientation,
expectedAngularVelocity,
testedYoFrameSO3TrajectoryPoint,
epsilon);
}
public void getVelocity(FrameVector linearVelocityToPack) {
linearVelocityToPack.setIncludingFrame(
ReferenceFrame.getWorldFrame(),
qd_x.getDoubleValue(),
qd_y.getDoubleValue(),
qd_z.getDoubleValue());
}
public void updateRobotLocationBasedOnMultisensePose(RigidBodyTransform headPose) {
RigidBodyTransform pelvisPose = new RigidBodyTransform();
RigidBodyTransform transformFromHeadToPelvis =
pelvisFrame.getTransformToDesiredFrame(headFrame);
pelvisPose.multiply(headPose, transformFromHeadToPelvis);
atomicPelvisPose.set(pelvisPose);
}
/**
* Set rotation axis in trajectoryFrame. Default axis is zUp in trajectoryFrame.
*
* @param circleCenterInTrajectoryFrame
* @param circleNormalInTrajectoryFrame
*/
public void updateCircleFrame(
Point3d circleCenterInTrajectoryFrame, Vector3d circleNormalInTrajectoryFrame) {
circleOrigin.set(circleCenterInTrajectoryFrame);
rotationAxis.set(circleNormalInTrajectoryFrame);
rotationAxis.normalize();
circleFrame.update();
}
public void getLinearAcceleration(FrameVector linearAccelerationToPack) {
linearAccelerationToPack.setIncludingFrame(
ReferenceFrame.getWorldFrame(),
qdd_x.getDoubleValue(),
qdd_y.getDoubleValue(),
qdd_z.getDoubleValue());
}
public QuadrupedXGaitStepStream(
QuadrupedPlanarVelocityInputProvider planarVelocityProvider,
QuadrupedXGaitSettingsInputProvider xGaitSettingsProvider,
QuadrupedReferenceFrames referenceFrames,
double controlDT,
DoubleYoVariable timestamp,
YoVariableRegistry parentRegistry) {
this.planarVelocityProvider = planarVelocityProvider;
this.xGaitSettingsProvider = xGaitSettingsProvider;
this.xGaitSettings = new QuadrupedXGaitSettings();
this.xGaitStepPlanner = new QuadrupedXGaitPlanner();
this.xGaitPreviewSteps = new ArrayList<>(NUMBER_OF_PREVIEW_STEPS);
this.xGaitCurrentSteps = new EndDependentList<>();
for (int i = 0; i < NUMBER_OF_PREVIEW_STEPS; i++) {
xGaitPreviewSteps.add(new QuadrupedTimedStep());
}
for (RobotEnd robotEnd : RobotEnd.values) {
xGaitCurrentSteps.set(robotEnd, new QuadrupedTimedStep());
}
this.supportCentroid = new FramePoint();
this.supportFrame = referenceFrames.getCenterOfFeetZUpFrameAveragingLowestZHeightsAcrossEnds();
this.bodyZUpFrame = referenceFrames.getBodyZUpFrame();
this.worldFrame = ReferenceFrame.getWorldFrame();
this.controlDT = controlDT;
this.timestamp = timestamp;
this.bodyOrientation = new FrameOrientation();
this.stepSequence =
new PreallocatedList<>(QuadrupedTimedStep.class, NUMBER_OF_PREVIEW_STEPS + 2);
if (parentRegistry != null) {
parentRegistry.addChild(registry);
}
}
public class CenterOfMassAccelerationCalculator {
private final FramePoint comLocation = new FramePoint(ReferenceFrame.getWorldFrame());
private final FrameVector linkLinearMomentumDot = new FrameVector(ReferenceFrame.getWorldFrame());
private final SpatialAccelerationCalculator spatialAccelerationCalculator;
private final RigidBody[] rigidBodies;
private final RigidBody base;
public CenterOfMassAccelerationCalculator(
RigidBody rootBody, SpatialAccelerationCalculator spatialAccelerationCalculator) {
this(
rootBody,
ScrewTools.computeSupportAndSubtreeSuccessors(rootBody),
spatialAccelerationCalculator);
}
public CenterOfMassAccelerationCalculator(
RigidBody base,
RigidBody[] rigidBodies,
SpatialAccelerationCalculator spatialAccelerationCalculator) {
this.spatialAccelerationCalculator = spatialAccelerationCalculator;
this.rigidBodies = rigidBodies;
this.base = base;
}
public void getCoMAcceleration(FrameVector comAccelerationToPack) {
boolean firstIteration = true;
double totalMass = 0.0;
for (RigidBody rigidBody : rigidBodies) {
double mass = rigidBody.getInertia().getMass();
rigidBody.getCoMOffset(comLocation);
spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(
linkLinearMomentumDot, base, rigidBody, comLocation);
linkLinearMomentumDot.scale(mass);
if (firstIteration) comAccelerationToPack.setIncludingFrame(linkLinearMomentumDot);
else comAccelerationToPack.add(linkLinearMomentumDot);
totalMass += mass;
firstIteration = false;
}
comAccelerationToPack.scale(1.0 / totalMass);
}
}
@DeployableTestMethod(estimatedDuration = 0.1)
@Test(timeout = 30000)
public void testConstructorsGettersAndSetters() {
QuadrupedSupportPolygon quadrupedSupportPolygon = new QuadrupedSupportPolygon();
QuadrantDependentList<Tuple3d> footPoints = new QuadrantDependentList<>();
footPoints.set(RobotQuadrant.HIND_LEFT, new Point3d(0.0, 0.0, 0.0));
footPoints.set(RobotQuadrant.HIND_RIGHT, new Point3d(1.0, 0.0, 0.0));
footPoints.set(RobotQuadrant.FRONT_LEFT, new Point3d(0.0, 1.0, 0.0));
footPoints.set(RobotQuadrant.FRONT_RIGHT, new Point3d(1.0, 1.0, 0.0));
QuadrantDependentList<FramePoint> framePoints = new QuadrantDependentList<>();
for (RobotQuadrant robotQuadrant : RobotQuadrant.values) {
framePoints.set(
robotQuadrant,
new FramePoint(ReferenceFrame.getWorldFrame(), footPoints.get(robotQuadrant)));
}
quadrupedSupportPolygon = new QuadrupedSupportPolygon(framePoints);
quadrupedSupportPolygon = new QuadrupedSupportPolygon(quadrupedSupportPolygon);
for (RobotQuadrant robotQuadrant : RobotQuadrant.values) {
quadrupedSupportPolygon.setFootstep(
robotQuadrant,
new FramePoint(ReferenceFrame.getWorldFrame(), footPoints.get(robotQuadrant)));
}
quadrupedSupportPolygon.printOutPolygon(getClass().getMethods()[0].getName());
for (RobotQuadrant robotQuadrant : RobotQuadrant.values) {
Point3d tuple3dToPack = new Point3d();
quadrupedSupportPolygon.getFootstep(robotQuadrant).get(tuple3dToPack);
assertEquals("Point not equal", footPoints.get(robotQuadrant), tuple3dToPack);
}
assertEquals(
"RefFrame getter not work",
ReferenceFrame.getWorldFrame(),
quadrupedSupportPolygon.getReferenceFrame());
System.out.println(quadrupedSupportPolygon.toString());
quadrupedSupportPolygon.changeFrame(ReferenceFrame.getWorldFrame());
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSetToNaN() throws Exception {
Random random = new Random(21651016L);
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
ReferenceFrame expectedFrame = worldFrame;
double expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
FrameOrientation expectedOrientation =
FrameOrientation.generateRandomFrameOrientation(random, expectedFrame);
FrameVector expectedAngularVelocity =
FrameVector.generateRandomFrameVector(random, expectedFrame);
String expectedNamePrefix = "test";
String expectedNameSuffix = "blop";
YoFrameSO3TrajectoryPoint testedYoFrameSO3TrajectoryPoint =
new YoFrameSO3TrajectoryPoint(
expectedNamePrefix,
expectedNameSuffix,
new YoVariableRegistry("schnoop"),
expectedFrame);
testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);
testedYoFrameSO3TrajectoryPoint.setToNaN();
assertTrue(Double.isNaN(testedYoFrameSO3TrajectoryPoint.getTime()));
assertTrue(testedYoFrameSO3TrajectoryPoint.getOrientation().containsNaN());
assertTrue(testedYoFrameSO3TrajectoryPoint.getAngularVelocity().containsNaN());
expectedFrame = ReferenceFrame.generateRandomReferenceFrame("blop", random, worldFrame);
testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(expectedFrame);
expectedTime = RandomTools.generateRandomDouble(random, 0.0, 1000.0);
expectedOrientation = FrameOrientation.generateRandomFrameOrientation(random, worldFrame);
expectedAngularVelocity = FrameVector.generateRandomFrameVector(random, worldFrame);
testedYoFrameSO3TrajectoryPoint.switchCurrentReferenceFrame(worldFrame);
testedYoFrameSO3TrajectoryPoint.registerReferenceFrame(worldFrame);
testedYoFrameSO3TrajectoryPoint.set(expectedTime, expectedOrientation, expectedAngularVelocity);
testedYoFrameSO3TrajectoryPoint.setToNaN(expectedFrame);
assertTrue(expectedFrame == testedYoFrameSO3TrajectoryPoint.getReferenceFrame());
assertTrue(Double.isNaN(testedYoFrameSO3TrajectoryPoint.getTime()));
assertTrue(testedYoFrameSO3TrajectoryPoint.getOrientation().containsNaN());
assertTrue(testedYoFrameSO3TrajectoryPoint.getAngularVelocity().containsNaN());
}
@Override
public void changeFrame(ReferenceFrame desiredFrame) {
if (desiredFrame != referenceFrame) {
referenceFrame.verifySameRoots(desiredFrame);
RigidBodyTransform referenceFrameTransformToRoot, rootTransformToDesiredFrame;
if ((referenceFrameTransformToRoot = referenceFrame.getTransformToRoot()) != null) {
geometryObject.applyTransform(referenceFrameTransformToRoot);
}
if ((rootTransformToDesiredFrame = desiredFrame.getInverseTransformToRoot()) != null) {
geometryObject.applyTransform(rootTransformToDesiredFrame);
}
referenceFrame = desiredFrame;
}
// otherwise: in the right frame already, so do nothing
}
@Override
public Footstep predictFootstepAfterDesiredFootstep(
RobotSide supportLegSide, Footstep desiredFootstep) {
RobotSide futureSwingLegSide = supportLegSide;
ReferenceFrame futureSupportAnkleFrame = desiredFootstep.getPoseReferenceFrame();
ReferenceFrame futureSupportAnkleZUpFrame =
new ZUpFrame(ReferenceFrame.getWorldFrame(), futureSupportAnkleFrame, "ankleZUp");
futureSupportAnkleZUpFrame.update();
computeDistanceAndAngleToDestination(
futureSupportAnkleZUpFrame, futureSwingLegSide, desiredDestination.getFramePoint2dCopy());
Matrix3d footToWorldRotation =
computeDesiredFootRotation(
angleToDestination.getDoubleValue(), futureSwingLegSide, futureSupportAnkleFrame);
FramePoint footstepPosition =
getDesiredFootstepPositionCopy(
futureSupportAnkleZUpFrame,
futureSupportAnkleFrame,
futureSwingLegSide,
desiredDestination.getFramePoint2dCopy(),
footToWorldRotation);
FrameOrientation footstepOrientation = new FrameOrientation(ReferenceFrame.getWorldFrame());
double[] yawPitchRoll = new double[3];
RotationTools.convertMatrixToYawPitchRoll(footToWorldRotation, yawPitchRoll);
footstepOrientation.setYawPitchRoll(yawPitchRoll);
FramePose footstepPose = new FramePose(footstepPosition, footstepOrientation);
footstepPose.changeFrame(ReferenceFrame.getWorldFrame());
PoseReferenceFrame poseReferenceFrame =
new PoseReferenceFrame("poseReferenceFrame", footstepPose);
ContactablePlaneBody foot = contactableBodies.get(futureSwingLegSide);
boolean trustHeight = true;
return new Footstep(
foot.getRigidBody(),
futureSwingLegSide,
foot.getSoleFrame(),
poseReferenceFrame,
trustHeight);
}
public class FullRobotModelRootJointRewinder implements RewoundListener {
private final YoVariableRegistry registry = new YoVariableRegistry(getClass().getSimpleName());
private final FullRobotModel fullRobotModel;
private final YoFrameVector yoRootJointTranslation =
new YoFrameVector("yoRootJointTranslation", ReferenceFrame.getWorldFrame(), registry);
private final Vector3d rootJointTranslation = new Vector3d();
private final YoFrameQuaternion yoRootJointRotation =
new YoFrameQuaternion("rootJointRotation", ReferenceFrame.getWorldFrame(), registry);
private final Quat4d rootJointRotation = new Quat4d();
public FullRobotModelRootJointRewinder(
FullRobotModel fullRobotModel, YoVariableRegistry parentRegistry) {
this.fullRobotModel = fullRobotModel;
parentRegistry.addChild(registry);
}
public void recordCurrentState() {
SixDoFJoint rootJoint = fullRobotModel.getRootJoint();
SixDoFJointReferenceFrame rootJointFrame = rootJoint.getFrameAfterJoint();
rootJointFrame.getTraslation(rootJointTranslation);
rootJointFrame.getRotation(rootJointRotation);
yoRootJointTranslation.set(rootJointTranslation);
yoRootJointRotation.set(rootJointRotation);
}
@Override
public void wasRewound() {
SixDoFJoint rootJoint = fullRobotModel.getRootJoint();
yoRootJointTranslation.get(rootJointTranslation);
rootJoint.setPosition(rootJointTranslation);
yoRootJointRotation.get(rootJointRotation);
rootJoint.setRotation(rootJointRotation);
}
}
private FramePose2d getCurrentMidFeetPose2dTheHardWayBecauseReferenceFramesDontUpdateProperly(
HumanoidFloatingRootJointRobot robot) {
FramePose midFeetPose = getRobotMidFeetPose(robot);
FramePose2d ret = new FramePose2d();
ret.setPoseIncludingFrame(
ReferenceFrame.getWorldFrame(),
midFeetPose.getX(),
midFeetPose.getY(),
midFeetPose.getYaw());
return ret;
}
private void computeOrientationStateOutputBlock() {
estimationFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame());
tempTransform.getRotation(rotationFromEstimationToWorld);
tempCenterOfMassPosition.setIncludingFrame(centerOfMassPositionPort.getData());
tempCenterOfMassPosition.changeFrame(estimationFrame);
ReferenceFrame referenceFrame =
referenceFrameMap.getFrameByName(
pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName());
tempFramePoint.setIncludingFrame(
referenceFrame,
pointPositionMeasurementInputPort.getData().getMeasurementPointInBodyFrame());
tempFramePoint.changeFrame(estimationFrame);
tempFramePoint.sub(tempCenterOfMassPosition);
tempFramePoint.scale(-1.0);
MatrixTools.toTildeForm(tempMatrix, tempFramePoint.getPoint());
tempMatrix.mul(rotationFromEstimationToWorld, tempMatrix);
MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort));
}
public DenseMatrix64F computeResidual() {
PointPositionDataObject data = pointPositionMeasurementInputPort.getData();
ReferenceFrame referenceFrame =
referenceFrameMap.getFrameByName(
pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName());
tempFramePoint.setIncludingFrame(referenceFrame, data.getMeasurementPointInBodyFrame());
tempFramePoint.changeFrame(ReferenceFrame.getWorldFrame());
residualVector.set(data.getMeasurementPointInWorldFrame());
residualVector.sub(tempFramePoint.getPoint());
MatrixTools.insertTuple3dIntoEJMLVector(residualVector, residual, 0);
return residual;
}
private QuadrupedSupportPolygon create3LegPolygon() {
QuadrantDependentList<Tuple3d> footPoints = new QuadrantDependentList<>();
footPoints.set(RobotQuadrant.HIND_LEFT, new Point3d(0.0, 0.0, 0.0));
footPoints.set(RobotQuadrant.HIND_RIGHT, new Point3d(1.0, 0.0, 0.0));
footPoints.set(RobotQuadrant.FRONT_RIGHT, new Point3d(1.0, 1.0, 0.0));
QuadrantDependentList<FramePoint> framePoints = new QuadrantDependentList<>();
for (RobotQuadrant robotQuadrant : footPoints.quadrants()) {
framePoints.set(
robotQuadrant,
new FramePoint(ReferenceFrame.getWorldFrame(), footPoints.get(robotQuadrant)));
}
return new QuadrupedSupportPolygon(framePoints);
}
private void sortDebrisFromCloserToFarther(ArrayList<DebrisData> debrisDataListToBeSorted) {
double currentDistanceToObject;
FramePoint currentObjectPosition = new FramePoint();
for (int i = 0; i < debrisDataListToBeSorted.size(); i++) {
DebrisData currentDebrisData = debrisDataListToBeSorted.get(i);
currentObjectPosition.changeFrame(ReferenceFrame.getWorldFrame());
currentObjectPosition.set(currentDebrisData.getGraspVectorPosition());
currentObjectPosition.changeFrame(fullRobotModel.getChest().getBodyFixedFrame());
currentDistanceToObject = currentObjectPosition.getX();
debrisDistanceMap.put(currentDistanceToObject, currentDebrisData);
}
sortedDebrisDataList.clear();
sortedDebrisDataList.addAll(debrisDistanceMap.values());
}
private FramePose2d getCurrentMidFeetPose2d(HumanoidReferenceFrames referenceFrames) {
robotDataReceiver.updateRobotModel();
referenceFrames.updateFrames();
ReferenceFrame midFeetFrame = referenceFrames.getMidFeetZUpFrame();
FramePose midFeetPose = new FramePose();
midFeetPose.setToZero(midFeetFrame);
midFeetPose.changeFrame(ReferenceFrame.getWorldFrame());
FramePose2d ret = new FramePose2d();
ret.setPoseIncludingFrame(
midFeetPose.getReferenceFrame(),
midFeetPose.getX(),
midFeetPose.getY(),
midFeetPose.getYaw());
return ret;
}
private FramePoint computeDesiredFootPosition(
RobotSide upcomingSwingLegSide,
ReferenceFrame upcomingSupportAnkleZUpFrame,
ReferenceFrame upcomingSupportAnkleFrame,
FrameVector2d desiredOffsetFromAnkle,
Matrix3d swingFootToWorldRotation) {
ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
desiredOffsetFromAnkle.changeFrame(upcomingSupportAnkleFrame);
FramePoint footstepPosition =
new FramePoint(
upcomingSupportAnkleFrame,
desiredOffsetFromAnkle.getX(),
desiredOffsetFromAnkle.getY(),
0.0);
footstepPosition.changeFrame(worldFrame);
return footstepPosition;
}
private FramePoint getDesiredFootstepPositionCopy(
ReferenceFrame supportAnkleZUpFrame,
ReferenceFrame supportAnkleFrame,
RobotSide swingLegSide,
FramePoint2d desiredDestination,
Matrix3d footToWorldRotation) {
FrameVector2d desiredOffsetFromAnkle =
computeDesiredOffsetFromSupportAnkle(
supportAnkleZUpFrame,
swingLegSide,
angleToDestination.getDoubleValue(),
distanceToDestination.getDoubleValue());
FramePoint footstepPosition =
computeDesiredFootPosition(
swingLegSide,
supportAnkleZUpFrame,
supportAnkleFrame,
desiredOffsetFromAnkle,
footToWorldRotation);
footstepPosition.changeFrame(ReferenceFrame.getWorldFrame());
return footstepPosition;
}
public class PointPositionMeasurementModelElement extends AbstractMeasurementModelElement {
public static final int SIZE = 3;
private final ControlFlowOutputPort<FramePoint> centerOfMassPositionPort;
private final ControlFlowOutputPort<FrameOrientation> orientationPort;
private final ControlFlowInputPort<PointPositionDataObject> pointPositionMeasurementInputPort;
private final ReferenceFrame estimationFrame;
private final DenseMatrix64F residual = new DenseMatrix64F(SIZE, 1);
private final Matrix3d rotationFromEstimationToWorld = new Matrix3d();
private final RigidBodyTransform tempTransform = new RigidBodyTransform();
private final Matrix3d tempMatrix = new Matrix3d();
private final FramePoint tempFramePoint = new FramePoint(ReferenceFrame.getWorldFrame());
private final Vector3d residualVector = new Vector3d();
private final AfterJointReferenceFrameNameMap referenceFrameMap;
private final boolean assumePerfectIMU;
public PointPositionMeasurementModelElement(
String name,
ControlFlowInputPort<PointPositionDataObject> pointPositionMeasurementInputPort,
ControlFlowOutputPort<FramePoint> centerOfMassPositionPort,
ControlFlowOutputPort<FrameOrientation> orientationPort,
ReferenceFrame estimationFrame,
AfterJointReferenceFrameNameMap referenceFrameMap,
boolean assumePerfectIMU,
YoVariableRegistry registry) {
super(SIZE, name, registry);
this.centerOfMassPositionPort = centerOfMassPositionPort;
this.orientationPort = orientationPort;
this.assumePerfectIMU = assumePerfectIMU;
if (assumePerfectIMU && orientationPort != null) throw new RuntimeException();
if (!assumePerfectIMU && orientationPort == null) throw new RuntimeException();
this.pointPositionMeasurementInputPort = pointPositionMeasurementInputPort;
this.estimationFrame = estimationFrame;
this.referenceFrameMap = referenceFrameMap;
if (assumePerfectIMU) initialize(SIZE, centerOfMassPositionPort);
else initialize(SIZE, centerOfMassPositionPort, orientationPort);
computeCenterOfMassPositionStateOutputBlock();
}
public void computeMatrixBlocks() {
if (!assumePerfectIMU) computeOrientationStateOutputBlock();
}
private void computeCenterOfMassPositionStateOutputBlock() {
CommonOps.setIdentity(getOutputMatrixBlock(centerOfMassPositionPort));
}
private final FramePoint tempCenterOfMassPosition = new FramePoint();
private void computeOrientationStateOutputBlock() {
estimationFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame());
tempTransform.getRotation(rotationFromEstimationToWorld);
tempCenterOfMassPosition.setIncludingFrame(centerOfMassPositionPort.getData());
tempCenterOfMassPosition.changeFrame(estimationFrame);
ReferenceFrame referenceFrame =
referenceFrameMap.getFrameByName(
pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName());
tempFramePoint.setIncludingFrame(
referenceFrame,
pointPositionMeasurementInputPort.getData().getMeasurementPointInBodyFrame());
tempFramePoint.changeFrame(estimationFrame);
tempFramePoint.sub(tempCenterOfMassPosition);
tempFramePoint.scale(-1.0);
MatrixTools.toTildeForm(tempMatrix, tempFramePoint.getPoint());
tempMatrix.mul(rotationFromEstimationToWorld, tempMatrix);
MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort));
}
public DenseMatrix64F computeResidual() {
PointPositionDataObject data = pointPositionMeasurementInputPort.getData();
ReferenceFrame referenceFrame =
referenceFrameMap.getFrameByName(
pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName());
tempFramePoint.setIncludingFrame(referenceFrame, data.getMeasurementPointInBodyFrame());
tempFramePoint.changeFrame(ReferenceFrame.getWorldFrame());
residualVector.set(data.getMeasurementPointInWorldFrame());
residualVector.sub(tempFramePoint.getPoint());
MatrixTools.insertTuple3dIntoEJMLVector(residualVector, residual, 0);
return residual;
}
public ControlFlowInputPort<PointPositionDataObject> getPointPositionMeasurementInputPort() {
return pointPositionMeasurementInputPort;
}
}
public class MomentumCalculatorTest {
private final ReferenceFrame world = ReferenceFrame.getWorldFrame();
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSingleRigidBodyTranslation() {
Random random = new Random(1766L);
RigidBody elevator = new RigidBody("elevator", world);
Vector3d jointAxis = RandomTools.generateRandomVector(random);
jointAxis.normalize();
PrismaticJoint joint =
ScrewTools.addPrismaticJoint("joint", elevator, new Vector3d(), jointAxis);
RigidBody body =
ScrewTools.addRigidBody(
"body",
joint,
RandomTools.generateRandomDiagonalMatrix3d(random),
random.nextDouble(),
new Vector3d());
joint.setQ(random.nextDouble());
joint.setQd(random.nextDouble());
Momentum momentum = computeMomentum(elevator, world);
momentum.changeFrame(world);
FrameVector linearMomentum =
new FrameVector(momentum.getExpressedInFrame(), momentum.getLinearPartCopy());
FrameVector angularMomentum =
new FrameVector(momentum.getExpressedInFrame(), momentum.getAngularPartCopy());
FrameVector linearMomentumCheck = new FrameVector(joint.getFrameBeforeJoint(), jointAxis);
linearMomentumCheck.scale(body.getInertia().getMass() * joint.getQd());
FrameVector angularMomentumCheck = new FrameVector(world);
double epsilon = 1e-9;
JUnitTools.assertTuple3dEquals(
linearMomentumCheck.getVector(), linearMomentum.getVector(), epsilon);
JUnitTools.assertTuple3dEquals(
angularMomentumCheck.getVector(), angularMomentum.getVector(), epsilon);
assertTrue(linearMomentum.length() > epsilon);
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testSingleRigidBodyRotation() {
Random random = new Random(1766L);
RigidBody elevator = new RigidBody("elevator", world);
Vector3d jointAxis = RandomTools.generateRandomVector(random);
jointAxis.normalize();
RigidBodyTransform transformToParent = new RigidBodyTransform();
transformToParent.setIdentity();
RevoluteJoint joint =
ScrewTools.addRevoluteJoint("joint", elevator, transformToParent, jointAxis);
RigidBody body =
ScrewTools.addRigidBody(
"body",
joint,
RandomTools.generateRandomDiagonalMatrix3d(random),
random.nextDouble(),
new Vector3d());
joint.setQ(random.nextDouble());
joint.setQd(random.nextDouble());
Momentum momentum = computeMomentum(elevator, world);
momentum.changeFrame(world);
FrameVector linearMomentum =
new FrameVector(momentum.getExpressedInFrame(), momentum.getLinearPartCopy());
FrameVector angularMomentum =
new FrameVector(momentum.getExpressedInFrame(), momentum.getAngularPartCopy());
FrameVector linearMomentumCheck = new FrameVector(world);
Matrix3d inertia = body.getInertia().getMassMomentOfInertiaPartCopy();
Vector3d angularMomentumCheckVector = new Vector3d(jointAxis);
angularMomentumCheckVector.scale(joint.getQd());
inertia.transform(angularMomentumCheckVector);
FrameVector angularMomentumCheck =
new FrameVector(body.getInertia().getExpressedInFrame(), angularMomentumCheckVector);
angularMomentumCheck.changeFrame(world);
double epsilon = 1e-9;
JUnitTools.assertTuple3dEquals(
linearMomentumCheck.getVector(), linearMomentum.getVector(), epsilon);
JUnitTools.assertTuple3dEquals(
angularMomentumCheck.getVector(), angularMomentum.getVector(), epsilon);
assertTrue(angularMomentum.length() > epsilon);
}
@ContinuousIntegrationTest(estimatedDuration = 0.0)
@Test(timeout = 30000)
public void testChainAgainstCentroidalMomentumMatrix() {
Random random = new Random(17679L);
ArrayList<RevoluteJoint> joints = new ArrayList<RevoluteJoint>();
RigidBody elevator = new RigidBody("elevator", world);
int nJoints = 10;
Vector3d[] jointAxes = new Vector3d[nJoints];
for (int i = 0; i < nJoints; i++) {
Vector3d jointAxis = RandomTools.generateRandomVector(random);
jointAxis.normalize();
jointAxes[i] = jointAxis;
}
ScrewTestTools.createRandomChainRobot("randomChain", joints, elevator, jointAxes, random);
InverseDynamicsJoint[] jointsArray = new RevoluteJoint[joints.size()];
joints.toArray(jointsArray);
ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("comFrame", world, elevator);
for (RevoluteJoint joint : joints) {
joint.setQ(random.nextDouble());
joint.setQd(random.nextDouble());
}
Momentum momentum = computeMomentum(elevator, centerOfMassFrame);
DenseMatrix64F momentumMatrix = new DenseMatrix64F(Momentum.SIZE, 1);
momentum.getMatrix(momentumMatrix);
CentroidalMomentumMatrix centroidalMomentumMatrix =
new CentroidalMomentumMatrix(elevator, centerOfMassFrame);
centroidalMomentumMatrix.compute();
DenseMatrix64F centroidalMomentumMatrixMatrix = centroidalMomentumMatrix.getMatrix();
DenseMatrix64F jointVelocitiesMatrix =
new DenseMatrix64F(ScrewTools.computeDegreesOfFreedom(jointsArray), 1);
ScrewTools.getJointVelocitiesMatrix(jointsArray, jointVelocitiesMatrix);
DenseMatrix64F momentumFromCentroidalMomentumMatrix = new DenseMatrix64F(Momentum.SIZE, 1);
CommonOps.mult(
centroidalMomentumMatrixMatrix,
jointVelocitiesMatrix,
momentumFromCentroidalMomentumMatrix);
double epsilon = 1e-9;
assertEquals(momentum.getExpressedInFrame(), centerOfMassFrame);
JUnitTools.assertMatrixEquals(momentumFromCentroidalMomentumMatrix, momentumMatrix, epsilon);
double norm = NormOps.normP2(momentumMatrix);
assertTrue(norm > epsilon);
}
private Momentum computeMomentum(RigidBody elevator, ReferenceFrame frame) {
elevator.updateFramesRecursively();
TwistCalculator twistCalculator = new TwistCalculator(world, elevator);
twistCalculator.compute();
MomentumCalculator momentumCalculator = new MomentumCalculator(twistCalculator);
Momentum momentum = new Momentum(frame);
momentumCalculator.computeAndPack(momentum);
return momentum;
}
}
private Point3d getPointInWorld(ReferenceFrame frame) {
tmpFramePoint.setToZero(frame);
tmpFramePoint.changeFrame(ReferenceFrame.getWorldFrame());
return tmpFramePoint.getPoint();
}
public class DesiredSteeringWheelProvider
implements PacketConsumer<SteeringWheelInformationPacket> {
private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
private final SideDependentList<AtomicReference<Point3d>> steeringWheelCenterAtomic =
new SideDependentList<>(
new AtomicReference<Point3d>(null), new AtomicReference<Point3d>(null));
private final SideDependentList<AtomicReference<Vector3d>> steeringWheelRotationAxisAtomic =
new SideDependentList<>(
new AtomicReference<Vector3d>(null), new AtomicReference<Vector3d>(null));
private final SideDependentList<AtomicReference<Vector3d>> steeringWheelZeroAxisAtomic =
new SideDependentList<>(
new AtomicReference<Vector3d>(null), new AtomicReference<Vector3d>(null));
private final SideDependentList<AtomicDouble> steeringWheelRadiusAtomic =
new SideDependentList<>(new AtomicDouble(Double.NaN), new AtomicDouble(Double.NaN));
private final SideDependentList<AtomicDouble> graspOffsetFromCotnrolFrameAtomic =
new SideDependentList<>(new AtomicDouble(Double.NaN), new AtomicDouble(Double.NaN));
private final SideDependentList<AtomicInteger> steeringWheelIdAtomic =
new SideDependentList<>(new AtomicInteger(-1), new AtomicInteger(-1));
private final SideDependentList<AtomicDouble> desiredAbsoluteSteeringAngle =
new SideDependentList<>(new AtomicDouble(0.0), new AtomicDouble(0.0));
private final SideDependentList<AtomicDouble> desiredSteeringSpeedAtomic =
new SideDependentList<>(new AtomicDouble(Double.NaN), new AtomicDouble(Double.NaN));
private final SideDependentList<AtomicBoolean> hasReceivedNewSteeringWheelInformation =
new SideDependentList<>(new AtomicBoolean(false), new AtomicBoolean(false));
private final SideDependentList<AtomicBoolean> hasReceivedNewDesiredSteeringAngle =
new SideDependentList<>(new AtomicBoolean(false), new AtomicBoolean(false));
private final PacketConsumer<DesiredSteeringAnglePacket> desiredSteeringAngleProvider;
private final HumanoidGlobalDataProducer globalDataProducer;
public DesiredSteeringWheelProvider(final HumanoidGlobalDataProducer globalDataProducer) {
this.globalDataProducer = globalDataProducer;
desiredSteeringAngleProvider =
new PacketConsumer<DesiredSteeringAnglePacket>() {
@Override
public void receivedPacket(DesiredSteeringAnglePacket packet) {
if (packet == null) return;
if (!PacketValidityChecker.validateDesiredSteeringAnglePacket(
packet, steeringWheelIdAtomic, globalDataProducer)) return;
RobotSide robotSide = packet.getRobotSide();
desiredAbsoluteSteeringAngle
.get(robotSide)
.set(packet.getDesiredAbsoluteSteeringAngle());
hasReceivedNewDesiredSteeringAngle.get(robotSide).set(true);
}
};
}
public PacketConsumer<DesiredSteeringAnglePacket> getDesiredSteeringAngleProvider() {
return desiredSteeringAngleProvider;
}
@Override
public void receivedPacket(SteeringWheelInformationPacket packet) {
if (packet == null) return;
if (!PacketValidityChecker.validateSteeringWheelInformationPacket(
packet, steeringWheelIdAtomic, globalDataProducer)) return;
RobotSide robotSide = packet.getRobotSide();
steeringWheelCenterAtomic.get(robotSide).set(packet.getSteeringWheelCenter());
steeringWheelRotationAxisAtomic.get(robotSide).set(packet.getSteeringWheelRotationAxis());
steeringWheelZeroAxisAtomic.get(robotSide).set(packet.getSteeringWheelZeroAxis());
steeringWheelRadiusAtomic.get(robotSide).set(packet.getSteeringWheelRadius());
graspOffsetFromCotnrolFrameAtomic.get(robotSide).set(packet.getGraspOffsetFromControlFrame());
desiredSteeringSpeedAtomic.get(robotSide).set(packet.getDesiredSteeringSpeed());
steeringWheelIdAtomic.get(robotSide).set(packet.getSteeringWheelId());
hasReceivedNewSteeringWheelInformation.get(robotSide).set(true);
}
public boolean checkForNewSteeringWheelInformation(RobotSide robotSide) {
return hasReceivedNewSteeringWheelInformation.get(robotSide).getAndSet(false);
}
public boolean checkForNewDesiredAbsoluteSteeringAngle(RobotSide robotSide) {
return hasReceivedNewDesiredSteeringAngle.get(robotSide).getAndSet(false);
}
public void getSteeringWheelPose(
RobotSide robotSide,
FramePoint centerToPack,
FrameVector rotationAxisToPack,
FrameVector zeroAxisToPack) {
centerToPack.setIncludingFrame(
worldFrame, steeringWheelCenterAtomic.get(robotSide).getAndSet(null));
rotationAxisToPack.setIncludingFrame(
worldFrame, steeringWheelRotationAxisAtomic.get(robotSide).getAndSet(null));
zeroAxisToPack.setIncludingFrame(
worldFrame, steeringWheelZeroAxisAtomic.get(robotSide).getAndSet(null));
}
public double getSteeringWheelRadius(RobotSide robotSide) {
return steeringWheelRadiusAtomic.get(robotSide).get();
}
public double getGraspOffsetFromControlFrame(RobotSide robotSide) {
return graspOffsetFromCotnrolFrameAtomic.get(robotSide).get();
}
public double getDesiredAbsoluteSteeringAngle(RobotSide robotSide) {
return desiredAbsoluteSteeringAngle.get(robotSide).get();
}
public double getDesiredSteeringSpeed(RobotSide robotSide) {
return desiredSteeringSpeedAtomic.get(robotSide).get();
}
}
/** @author twan Date: 6/12/13 */
public class CirclePoseTrajectoryGenerator implements PoseTrajectoryGenerator {
private static final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame();
private final YoVariableRegistry registry;
private final DoubleYoVariable currentTime;
private final YoPolynomial anglePolynomial;
private final DoubleProvider desiredTrajectoryTimeProvider;
private final DoubleYoVariable desiredTrajectoryTime;
private final DoubleYoVariable initialRadius;
private final DoubleYoVariable initialZ;
private final DoubleYoVariable initialAngle;
private final DoubleYoVariable currentRelativeAngle;
private final BooleanYoVariable isCurrentAngleBeingAdjusted;
private final DoubleYoVariable maximumAngleTrackingErrorTolerated;
private final DoubleYoVariable currentControlledFrameRelativeAngle;
private final DoubleYoVariable currentAngleTrackingError;
private final DoubleYoVariable currentAdjustedRelativeAngle;
private final DoubleYoVariable desiredRotationAngle;
private final DoubleYoVariable finalAngle;
private final FramePoint initialPosition = new FramePoint();
private final FramePoint currentPosition = new FramePoint();
private final FramePoint finalPosition = new FramePoint();
private final FrameOrientation initialOrientation = new FrameOrientation();
private final FrameOrientation finalOrientation = new FrameOrientation();
private final FrameOrientation currentOrientation = new FrameOrientation();
private final FrameVector currentVelocity = new FrameVector();
private final FrameVector currentAcceleration = new FrameVector();
private final FrameVector currentAngularVelocity = new FrameVector();
private final FrameVector currentAngularAcceleration = new FrameVector();
private final YoFramePoint yoInitialPosition;
private final YoFramePoint yoFinalPosition;
private final YoFramePoint yoCurrentPosition;
private final YoFrameVector yoCurrentVelocity;
private final YoFrameVector yoCurrentAcceleration;
private final YoFrameQuaternion yoInitialOrientation;
private final YoFrameQuaternion yoFinalOrientation;
private final YoFrameQuaternion yoCurrentOrientation;
private final YoFrameVector yoCurrentAngularVelocity;
private final YoFrameVector yoCurrentAngularAcceleration;
private final YoFramePoint yoInitialPositionWorld;
private final YoFramePoint yoFinalPositionWorld;
private final YoFramePoint yoCurrentPositionWorld;
private final YoFramePoint yoCurrentAdjustedPositionWorld;
private boolean rotateHandAngleAboutAxis = false;
private boolean visualize = true;
private final BagOfBalls bagOfBalls;
private final FramePoint ballPosition = new FramePoint();
private final int numberOfBalls = 50;
private final YoFramePoint circleOrigin;
private final YoFrameVector rotationAxis;
private final ReferenceFrame circleFrame;
private final ReferenceFrame trajectoryFrame;
private ReferenceFrame controlledFrame;
private final PoseReferenceFrame tangentialCircleFrame;
private final FramePose tangentialCircleFramePose = new FramePose();
private final YoFramePose yoTangentialCircleFramePose;
/**
* Use a BooleanYoVariable to hide and show visualization with a VariableChangedListener, so it is
* still working in playback mode.
*/
private final BooleanYoVariable showViz;
public CirclePoseTrajectoryGenerator(
String namePrefix,
ReferenceFrame trajectoryFrame,
DoubleProvider trajectoryTimeProvider,
YoVariableRegistry parentRegistry,
YoGraphicsListRegistry yoGraphicsListRegistry) {
this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName());
this.desiredTrajectoryTime = new DoubleYoVariable(namePrefix + "TrajectoryTime", registry);
this.currentTime = new DoubleYoVariable(namePrefix + "Time", registry);
// this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 2,
// registry);
this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 4, registry);
// this.anglePolynomial = new YoPolynomial(namePrefix + "ParameterPolynomial", 6,
// registry);
this.desiredTrajectoryTimeProvider = trajectoryTimeProvider;
this.trajectoryFrame = trajectoryFrame;
initialRadius = new DoubleYoVariable(namePrefix + "Radius", registry);
initialZ = new DoubleYoVariable(namePrefix + "ZPosition", registry);
initialAngle = new DoubleYoVariable(namePrefix + "InitialAngle", registry);
finalAngle = new DoubleYoVariable(namePrefix + "FinalAngle", registry);
isCurrentAngleBeingAdjusted =
new BooleanYoVariable(namePrefix + "IsCurrentAngleBeingAdjusted", registry);
maximumAngleTrackingErrorTolerated =
new DoubleYoVariable(namePrefix + "MaxAngleTrackingErrorTolerated", registry);
maximumAngleTrackingErrorTolerated.set(Math.toRadians(30.0));
currentControlledFrameRelativeAngle =
new DoubleYoVariable(namePrefix + "CurrentControlledFrameAngle", registry);
currentAngleTrackingError =
new DoubleYoVariable(namePrefix + "CurrentAngleTrackingError", registry);
currentAdjustedRelativeAngle =
new DoubleYoVariable(namePrefix + "CurrentAdjustedRelativeAngle", registry);
desiredRotationAngle = new DoubleYoVariable(namePrefix + "DesiredRotationAngle", registry);
currentRelativeAngle = new DoubleYoVariable(namePrefix + "CurrentRelativeAngle", registry);
yoInitialPosition = new YoFramePoint(namePrefix + "InitialPosition", trajectoryFrame, registry);
yoFinalPosition = new YoFramePoint(namePrefix + "FinalPosition", trajectoryFrame, registry);
yoCurrentPosition = new YoFramePoint(namePrefix + "CurrentPosition", trajectoryFrame, registry);
yoCurrentVelocity =
new YoFrameVector(namePrefix + "CurrentVelocity", trajectoryFrame, registry);
yoCurrentAcceleration =
new YoFrameVector(namePrefix + "CurrentAcceleration", trajectoryFrame, registry);
yoInitialOrientation =
new YoFrameQuaternion(namePrefix + "InitialOrientation", trajectoryFrame, registry);
yoFinalOrientation =
new YoFrameQuaternion(namePrefix + "FinalOrientation", trajectoryFrame, registry);
yoCurrentOrientation =
new YoFrameQuaternion(namePrefix + "CurrentOrientation", trajectoryFrame, registry);
yoCurrentAngularVelocity =
new YoFrameVector(namePrefix + "CurrentAngularVelocity", trajectoryFrame, registry);
yoCurrentAngularAcceleration =
new YoFrameVector(namePrefix + "CurrentAngularAcceleration", trajectoryFrame, registry);
yoInitialPositionWorld =
new YoFramePoint(namePrefix + "InitialPositionWorld", worldFrame, registry);
yoFinalPositionWorld =
new YoFramePoint(namePrefix + "FinalPositionWorld", worldFrame, registry);
yoCurrentPositionWorld =
new YoFramePoint(namePrefix + "CurrentPositionWorld", worldFrame, registry);
yoCurrentAdjustedPositionWorld =
new YoFramePoint(namePrefix + "CurrentAdjustedPositionWorld", worldFrame, registry);
circleOrigin = new YoFramePoint(namePrefix + "CircleOrigin", trajectoryFrame, registry);
rotationAxis = new YoFrameVector(namePrefix + "RotationAxis", trajectoryFrame, registry);
rotationAxis.set(0.0, 0.0, 1.0);
circleFrame =
new ReferenceFrame("CircleFrame", trajectoryFrame) {
private static final long serialVersionUID = 9102217353690768074L;
private final Vector3d localTranslation = new Vector3d();
private final Vector3d localRotationAxis = new Vector3d();
private final AxisAngle4d localAxisAngle = new AxisAngle4d();
@Override
protected void updateTransformToParent(RigidBodyTransform transformToParent) {
circleOrigin.get(localTranslation);
rotationAxis.get(localRotationAxis);
GeometryTools.getRotationBasedOnNormal(localAxisAngle, localRotationAxis);
transformToParent.set(localAxisAngle, localTranslation);
}
};
tangentialCircleFrame = new PoseReferenceFrame("TangentialCircleFrame", circleFrame);
yoTangentialCircleFramePose =
new YoFramePose(namePrefix + "TangentialCircleFramePose", worldFrame, registry);
if (this.visualize && yoGraphicsListRegistry != null) {
final YoGraphicPosition currentPositionViz =
new YoGraphicPosition(
namePrefix + "CurrentPosition", yoCurrentPositionWorld, 0.025, YoAppearance.Blue());
final YoGraphicPosition currentAdjustedPositionViz =
new YoGraphicPosition(
namePrefix + "CurrentAdjustedPosition",
yoCurrentAdjustedPositionWorld,
0.023,
YoAppearance.Gold());
final YoGraphicPosition initialPositionViz =
new YoGraphicPosition(
namePrefix + "InitialPosition",
yoInitialPositionWorld,
0.02,
YoAppearance.BlueViolet());
final YoGraphicPosition finalPositionViz =
new YoGraphicPosition(
namePrefix + "FinalPosition", yoFinalPositionWorld, 0.02, YoAppearance.Red());
final YoGraphicCoordinateSystem tangentialFrameViz =
new YoGraphicCoordinateSystem(
namePrefix + "TangentialFrame",
yoTangentialCircleFramePose,
0.2,
YoAppearance.Pink());
YoGraphicsList yoGraphicsList = new YoGraphicsList(namePrefix + "CircleTraj");
yoGraphicsList.add(currentPositionViz);
yoGraphicsList.add(currentAdjustedPositionViz);
yoGraphicsList.add(initialPositionViz);
yoGraphicsList.add(finalPositionViz);
yoGraphicsList.add(tangentialFrameViz);
yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList);
bagOfBalls =
new BagOfBalls(
numberOfBalls, 0.01, yoGraphicsList.getLabel(), registry, yoGraphicsListRegistry);
showViz = new BooleanYoVariable(namePrefix + "ShowViz", registry);
showViz.addVariableChangedListener(
new VariableChangedListener() {
public void variableChanged(YoVariable<?> v) {
boolean visible = showViz.getBooleanValue();
currentPositionViz.setVisible(visible);
currentAdjustedPositionViz.setVisible(visible);
initialPositionViz.setVisible(visible);
finalPositionViz.setVisible(visible);
tangentialFrameViz.setVisible(visible);
bagOfBalls.setVisible(visible);
}
});
showViz.notifyVariableChangedListeners();
} else {
visualize = false;
bagOfBalls = null;
showViz = null;
}
parentRegistry.addChild(registry);
}
/**
* Set rotation axis in trajectoryFrame. Default axis is zUp in trajectoryFrame.
*
* @param circleCenterInTrajectoryFrame
* @param circleNormalInTrajectoryFrame
*/
public void updateCircleFrame(
Point3d circleCenterInTrajectoryFrame, Vector3d circleNormalInTrajectoryFrame) {
circleOrigin.set(circleCenterInTrajectoryFrame);
rotationAxis.set(circleNormalInTrajectoryFrame);
rotationAxis.normalize();
circleFrame.update();
}
public void updateCircleFrame(FramePoint circleCenter, FrameVector circleNormal) {
circleOrigin.setAndMatchFrame(circleCenter);
rotationAxis.setAndMatchFrame(circleNormal);
rotationAxis.normalize();
circleFrame.update();
}
public void setMaximumAngleTrackingErrorTolerated(double maximumAngle) {
this.maximumAngleTrackingErrorTolerated.set(maximumAngle);
}
public void setControlledFrame(ReferenceFrame controlledFrame) {
this.controlledFrame = controlledFrame;
}
public void setDesiredRotationAngle(double desiredRotationAngle) {
this.desiredRotationAngle.set(desiredRotationAngle);
}
public void setInitialPoseToMatchReferenceFrame(ReferenceFrame referenceFrame) {
initialPosition.setToZero(referenceFrame);
initialOrientation.setToZero(referenceFrame);
initialPosition.changeFrame(trajectoryFrame);
initialOrientation.changeFrame(trajectoryFrame);
yoInitialPosition.set(initialPosition);
yoInitialOrientation.set(initialOrientation);
}
public void setInitialPose(FramePose initialPose) {
initialPose.changeFrame(trajectoryFrame);
initialPose.getPoseIncludingFrame(initialPosition, initialOrientation);
yoInitialPosition.set(initialPosition);
yoInitialOrientation.set(initialOrientation);
}
public void setControlHandAngleAboutAxis(boolean controlIfTrue) {
rotateHandAngleAboutAxis = controlIfTrue;
}
public void initialize() {
currentTime.set(0.0);
desiredTrajectoryTime.set(desiredTrajectoryTimeProvider.getValue());
yoInitialPosition.getFrameTupleIncludingFrame(initialPosition);
initialPosition.changeFrame(circleFrame);
if (rotateHandAngleAboutAxis)
rotateInitialOrientation(finalOrientation, desiredRotationAngle.getDoubleValue());
else finalOrientation.setIncludingFrame(initialOrientation);
finalOrientation.changeFrame(trajectoryFrame);
yoFinalOrientation.set(finalOrientation);
double x = initialPosition.getX();
double y = initialPosition.getY();
initialZ.set(initialPosition.getZ());
initialRadius.set(Math.sqrt(x * x + y * y));
initialAngle.set(Math.atan2(y, x));
finalAngle.set(initialAngle.getDoubleValue() + desiredRotationAngle.getDoubleValue());
// anglePolynomial.setLinear(0.0, desiredTrajectoryTime.getDoubleValue(),
// initialAngle.getDoubleValue(), finalAngle.getDoubleValue());
anglePolynomial.setCubic(
0.0,
desiredTrajectoryTime.getDoubleValue(),
initialAngle.getDoubleValue(),
0.0,
finalAngle.getDoubleValue(),
0.0);
// anglePolynomial.setQuintic(0.0, desiredTrajectoryTime.getDoubleValue(),
// initialAngle.getDoubleValue(), 0.0, 0.0, finalAngle.getDoubleValue(), 0.0, 0.0);
double xFinal = initialRadius.getDoubleValue() * Math.cos(finalAngle.getDoubleValue());
double yFinal = initialRadius.getDoubleValue() * Math.sin(finalAngle.getDoubleValue());
double zFinal = initialZ.getDoubleValue();
finalPosition.setIncludingFrame(circleFrame, xFinal, yFinal, zFinal);
yoFinalPosition.setAndMatchFrame(finalPosition);
rotateInitialOrientation(finalOrientation, finalAngle.getDoubleValue());
currentAngleTrackingError.set(0.0);
currentControlledFrameRelativeAngle.set(initialAngle.getDoubleValue());
updateTangentialCircleFrame();
if (visualize) visualizeTrajectory();
}
public void compute(double time) {
compute(time, true);
}
public void compute(double time, boolean adjustAngle) {
this.currentTime.set(time);
time = MathTools.clipToMinMax(time, 0.0, desiredTrajectoryTime.getDoubleValue());
anglePolynomial.compute(time);
double angle = anglePolynomial.getPosition();
double angleDot = anglePolynomial.getVelocity();
double angleDDot = anglePolynomial.getAcceleration();
double cos = Math.cos(angle);
double sin = Math.sin(angle);
double r = initialRadius.getDoubleValue();
double x = r * cos;
double y = r * sin;
double z = initialZ.getDoubleValue();
currentPosition.setIncludingFrame(circleFrame, x, y, z);
yoCurrentPositionWorld.setAndMatchFrame(currentPosition);
currentRelativeAngle.set(
computeAngleDifferenceMinusPiToPi(angle, initialAngle.getDoubleValue()));
if (adjustAngle)
currentAdjustedRelativeAngle.set(
adjustCurrentDesiredRelativeAngle(currentRelativeAngle.getDoubleValue()));
else currentAdjustedRelativeAngle.set(currentRelativeAngle.getDoubleValue());
angle =
trimAngleMinusPiToPi(
currentAdjustedRelativeAngle.getDoubleValue() + initialAngle.getDoubleValue());
if (isDone()) {
angle = finalAngle.getDoubleValue();
angleDot = 0.0;
angleDDot = 0.0;
}
cos = Math.cos(angle);
sin = Math.sin(angle);
x = r * cos;
y = r * sin;
double xDot = -r * sin * angleDot;
double yDot = x * angleDot;
double zDot = 0.0;
double xDDot = -r * cos * angleDot * angleDot - y * angleDDot;
double yDDot = xDot * angleDot + x * angleDDot;
double zDDot = 0.0;
currentPosition.setIncludingFrame(circleFrame, x, y, z);
currentVelocity.setIncludingFrame(circleFrame, xDot, yDot, zDot);
currentAcceleration.setIncludingFrame(circleFrame, xDDot, yDDot, zDDot);
if (rotateHandAngleAboutAxis) {
rotateInitialOrientation(currentOrientation, angle - initialAngle.getDoubleValue());
currentAngularVelocity.setIncludingFrame(circleFrame, 0.0, 0.0, angleDot);
currentAngularAcceleration.setIncludingFrame(circleFrame, 0.0, 0.0, angleDDot);
} else {
currentOrientation.setIncludingFrame(initialOrientation);
currentAngularVelocity.setIncludingFrame(circleFrame, 0.0, 0.0, 0.0);
currentAngularAcceleration.setIncludingFrame(circleFrame, 0.0, 0.0, 0.0);
}
yoCurrentPosition.setAndMatchFrame(currentPosition);
yoCurrentAdjustedPositionWorld.setAndMatchFrame(currentPosition);
yoCurrentVelocity.setAndMatchFrame(currentVelocity);
yoCurrentAcceleration.setAndMatchFrame(currentAcceleration);
currentOrientation.changeFrame(trajectoryFrame);
yoCurrentOrientation.set(currentOrientation);
yoCurrentAngularVelocity.setAndMatchFrame(currentAngularVelocity);
yoCurrentAngularAcceleration.setAndMatchFrame(currentAngularAcceleration);
updateTangentialCircleFrame();
}
private void updateTangentialCircleFrame() {
if (controlledFrame != null) {
tangentialCircleFramePose.setToZero(controlledFrame);
} else {
tangentialCircleFramePose.setToZero(currentPosition.getReferenceFrame());
tangentialCircleFramePose.setPosition(currentPosition);
}
tangentialCircleFramePose.changeFrame(circleFrame);
double x = tangentialCircleFramePose.getX();
double y = tangentialCircleFramePose.getY();
double yaw = trimAngleMinusPiToPi(Math.PI / 2.0 + Math.atan2(y, x));
tangentialCircleFramePose.setOrientation(yaw, 0.0, 0.0);
tangentialCircleFrame.setPoseAndUpdate(tangentialCircleFramePose);
yoTangentialCircleFramePose.setAndMatchFrame(tangentialCircleFramePose);
}
private final FramePoint currentControlledFramePosition = new FramePoint();
private double adjustCurrentDesiredRelativeAngle(double currentDesiredRelativeAngle) {
if (controlledFrame == null) {
isCurrentAngleBeingAdjusted.set(false);
return currentDesiredRelativeAngle;
}
currentControlledFramePosition.setToZero(controlledFrame);
currentControlledFramePosition.changeFrame(circleFrame);
double x = currentControlledFramePosition.getX();
double y = currentControlledFramePosition.getY();
currentControlledFrameRelativeAngle.set(
computeAngleDifferenceMinusPiToPi(Math.atan2(y, x), initialAngle.getDoubleValue()));
currentAngleTrackingError.set(
computeAngleDifferenceMinusPiToPi(
currentDesiredRelativeAngle, currentControlledFrameRelativeAngle.getDoubleValue()));
if (computeAngleDifferenceMinusPiToPi(
currentAngleTrackingError.getDoubleValue(),
maximumAngleTrackingErrorTolerated.getDoubleValue())
> 0.0) {
isCurrentAngleBeingAdjusted.set(true);
return trimAngleMinusPiToPi(
currentControlledFrameRelativeAngle.getDoubleValue()
+ maximumAngleTrackingErrorTolerated.getDoubleValue());
} else if (trimAngleMinusPiToPi(
currentAngleTrackingError.getDoubleValue()
+ maximumAngleTrackingErrorTolerated.getDoubleValue())
< 0.0) {
isCurrentAngleBeingAdjusted.set(true);
return computeAngleDifferenceMinusPiToPi(
currentControlledFrameRelativeAngle.getDoubleValue(),
maximumAngleTrackingErrorTolerated.getDoubleValue());
} else {
isCurrentAngleBeingAdjusted.set(false);
return currentDesiredRelativeAngle;
}
}
private final RigidBodyTransform axisRotationTransform = new RigidBodyTransform();
private void rotateInitialOrientation(
FrameOrientation orientationToPack, double angleFromInitialOrientation) {
initialOrientation.changeFrame(circleFrame);
orientationToPack.setIncludingFrame(initialOrientation);
axisRotationTransform.rotZ(angleFromInitialOrientation);
orientationToPack.applyTransform(axisRotationTransform);
}
private void visualizeTrajectory() {
yoInitialPosition.getFrameTupleIncludingFrame(initialPosition);
yoInitialPositionWorld.setAndMatchFrame(initialPosition);
yoFinalPosition.getFrameTupleIncludingFrame(finalPosition);
yoFinalPositionWorld.setAndMatchFrame(finalPosition);
for (int i = 0; i < numberOfBalls; i++) {
double t = (double) i / ((double) numberOfBalls - 1) * desiredTrajectoryTime.getDoubleValue();
compute(t, false);
yoCurrentPosition.getFrameTupleIncludingFrame(ballPosition);
ballPosition.changeFrame(worldFrame);
bagOfBalls.setBallLoop(ballPosition);
}
reset();
}
private void reset() {
compute(0.0);
}
public double getCurrentAngularDisplacement() {
return currentRelativeAngle.getDoubleValue();
}
public ReferenceFrame getTangentialCircleFrame() {
return tangentialCircleFrame;
}
public boolean isDone() {
return currentTime.getDoubleValue() >= desiredTrajectoryTime.getDoubleValue()
&& !isCurrentAngleBeingAdjusted.getBooleanValue();
}
public ReferenceFrame getCircleFrame() {
return circleFrame;
}
public void get(FramePoint positionToPack) {
yoCurrentAdjustedPositionWorld.getFrameTupleIncludingFrame(positionToPack);
}
public void packVelocity(FrameVector velocityToPack) {
yoCurrentVelocity.getFrameTupleIncludingFrame(velocityToPack);
}
public void packAcceleration(FrameVector accelerationToPack) {
yoCurrentAcceleration.getFrameTupleIncludingFrame(accelerationToPack);
}
public void get(FrameOrientation orientationToPack) {
yoCurrentOrientation.getFrameOrientationIncludingFrame(orientationToPack);
}
public void packAngularVelocity(FrameVector angularVelocityToPack) {
yoCurrentAngularVelocity.getFrameTupleIncludingFrame(angularVelocityToPack);
}
public void packAngularAcceleration(FrameVector angularAccelerationToPack) {
yoCurrentAngularAcceleration.getFrameTupleIncludingFrame(angularAccelerationToPack);
}
@Override
public void get(FramePose framePoseToPack) {
yoCurrentAdjustedPositionWorld.getFrameTupleIncludingFrame(currentPosition);
yoCurrentOrientation.getFrameOrientationIncludingFrame(currentOrientation);
framePoseToPack.setPoseIncludingFrame(currentPosition, currentOrientation);
}
public void packLinearData(
FramePoint positionToPack, FrameVector velocityToPack, FrameVector accelerationToPack) {
get(positionToPack);
packVelocity(velocityToPack);
packAcceleration(accelerationToPack);
}
public void packAngularData(
FrameOrientation orientationToPack,
FrameVector angularVelocityToPack,
FrameVector angularAccelerationToPack) {
get(orientationToPack);
packAngularVelocity(angularVelocityToPack);
packAngularAcceleration(angularAccelerationToPack);
}
public void showVisualization() {
if (!visualize) return;
showViz.set(true);
}
public void hideVisualization() {
if (!visualize) return;
showViz.set(false);
}
}