public void setInitialConditions(FramePoint initialPosition) { this.initialPosition.setAndMatchFrame(initialPosition); }
/** * Put the constant CMP's on the footsteps. * * @param constantCMPsToPack List that will be packed with the constant CMP locations * @param footstepList List containing the footsteps * @param firstFootstepIndex Integer describing the index of the first footstep to consider when laying out the CMP's * @param lastFootstepIndex Integer describing the index of the last footstep to consider when laying out the CMP's */ public static void computeConstantCMPsOnFeet(List<YoFramePoint> constantCMPsToPack, List<FramePoint> footstepList, int firstFootstepIndex, int lastFootstepIndex) { for (int i = firstFootstepIndex; i <= lastFootstepIndex; i++) { YoFramePoint constantCMP = constantCMPsToPack.get(i); // Put the constant CMP at the footstep location constantCMP.setAndMatchFrame(footstepList.get(i)); } }
public void setAndMatchFrame(FramePoint framePoint, FrameOrientation frameOrientation) { boolean notifyListeners = true; position.setAndMatchFrame(framePoint, notifyListeners); orientation.setAndMatchFrame(frameOrientation, notifyListeners); }
public void setInitialConditions(FramePoint initialPosition, FrameVector initialVelocity) { this.initialPosition.setAndMatchFrame(initialPosition); this.initialVelocity.setAndMatchFrame(initialVelocity); }
public void setFinalConditions(FramePoint finalPosition, FrameVector finalVelocity) { this.finalPosition.setAndMatchFrame(finalPosition); this.finalVelocity.setAndMatchFrame(finalVelocity); }
public void setAndMatchFrame(FramePoint framePoint, FrameOrientation frameOrientation) { boolean notifyListeners = true; position.setAndMatchFrame(framePoint, notifyListeners); orientation.setAndMatchFrame(frameOrientation, notifyListeners); }
@Override public void variableChanged(YoVariable<?> v) { getFrameTupleIncludingFrame(localFramePoint); point.setAndMatchFrame(localFramePoint); } });
public void setFinalConditions(FramePoint finalPosition, FrameVector finalVelocity) { this.finalPosition.setAndMatchFrame(finalPosition); this.finalVelocity.setAndMatchFrame(finalVelocity); }
/** * Put the constant CMP in the middle of the two given footsteps. * @param constantCMPToPack YoFramePoint that will be packed with the constant CMP location * @param firstFootstep FramePoint holding the position of the first footstep * @param secondFootstep FramePoint holding the position of the second footstep */ public static void putConstantCMPBetweenFeet(YoFramePoint constantCMPToPack, FramePoint firstFootstep, FramePoint secondFootstep) { constantCMPToPack.setAndMatchFrame(firstFootstep); constantCMPToPack.add(secondFootstep); constantCMPToPack.scale(0.5); }
public void updateCircleFrame(FramePoint circleCenter, FrameVector circleNormal) { circleOrigin.setAndMatchFrame(circleCenter); rotationAxis.setAndMatchFrame(circleNormal); rotationAxis.normalize(); circleFrame.update(); }
public void setDesiredHandPose(RobotSide robotSide, FramePoint desiredHandPosition, FrameOrientation desiredHandOrientation) { yoDesiredHandPositions.get(robotSide).setAndMatchFrame(desiredHandPosition); yoDesiredHandOrientations.get(robotSide).setAndMatchFrame(desiredHandOrientation); }
/** * Update the basics: capture point, omega0, and the support polygons. */ public void update() { centerOfMassPosition.setToZero(centerOfMassFrame); yoCenterOfMass.setAndMatchFrame(centerOfMassPosition); double omega0 = momentumBasedController.getOmega0(); CapturePointTools.computeDesiredCentroidalMomentumPivot(yoDesiredCapturePoint, yoDesiredICPVelocity, omega0, yoPerfectCMP); icpPlanner.getFinalDesiredCapturePointPosition(yoFinalDesiredICP); }
private void updatePositionVisualization() { yoDesiredLinearAcceleration.setAndMatchFrame(desiredLinearAcceleration); accelerationControlModule.getBodyFixedPoint(tempPosition); yoCurrentPosition.setAndMatchFrame(tempPosition); accelerationControlModule.getBodyFixedPointCurrentLinearVelocity(tempLinearVelocity); yoCurrentLinearVelocity.setAndMatchFrame(tempLinearVelocity); }
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 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) * trajectoryTime.getDoubleValue(); compute(t, false); yoCurrentPosition.getFrameTupleIncludingFrame(ballPosition); ballPosition.changeFrame(worldFrame); bagOfBalls.setBallLoop(ballPosition); } reset(); }
public void setFinalPoseWithoutFinalVelocity(FramePoint finalPosition, FrameOrientation finalOrientation) { this.finalPosition.set(finalPosition); this.finalOrientation.set(finalOrientation); finalPositionForViz.setAndMatchFrame(finalPosition); finalOrientationForViz.setAndMatchFrame(finalOrientation); this.finalVelocity.setToZero(); this.finalAngularVelocity.setToZero(); }
private void updatePositionVisualization() { desiredSpatialAcceleration.getLinearPart(desiredLinearAcceleration); yoDesiredLinearAcceleration.setAndMatchFrame(desiredLinearAcceleration); tempPosition.setToZero(accelerationControlModule.getTrackingFrame()); yoCurrentPosition.setAndMatchFrame(tempPosition); accelerationControlModule.getEndEffectorCurrentLinearVelocity(tempLinearVelocity); yoCurrentLinearVelocity.setAndMatchFrame(tempLinearVelocity); }
public void submitFeedbackControlCommand(PointFeedbackControlCommand command) { if (command.getEndEffector() != endEffector) throw new RuntimeException("Wrong end effector - received: " + command.getEndEffector() + ", expected: " + endEffector); base = command.getBase(); output.set(command.getPointAccelerationCommand()); accelerationControlModule.setGains(command.getGains()); command.getBodyFixedPointIncludingFrame(tempPosition); accelerationControlModule.setPointToControl(tempPosition); command.getIncludingFrame(tempPosition, tempLinearVelocity, feedForwardLinearAcceleration); yoDesiredPosition.setAndMatchFrame(tempPosition); yoDesiredLinearVelocity.setAndMatchFrame(tempLinearVelocity); yoFeedForwardLinearAcceleration.setAndMatchFrame(feedForwardLinearAcceleration); }
public void updateSteeringWheel(FramePoint center, FrameVector rotationAxis, FrameVector zeroAxis) { steeringWheelCenter.setAndMatchFrame(center); steeringWheelRotationAxis.setAndMatchFrame(rotationAxis); steeringWheelRotationAxis.normalize(); steeringWheelZeroAxis.setAndMatchFrame(zeroAxis); steeringWheelZeroAxis.normalize(); steeringWheelFrame.update(); steeringWheelFramePose.setToZero(steeringWheelFrame); steeringWheelFramePose.changeFrame(worldFrame); yoSteeringWheelFramePose.set(steeringWheelFramePose); }
public void submitFeedbackControlCommand(SpatialFeedbackControlCommand command) { if (command.getEndEffector() != endEffector) throw new RuntimeException("Wrong end effector - received: " + command.getEndEffector() + ", expected: " + endEffector); base = command.getBase(); output.set(command.getSpatialAccelerationCommand()); accelerationControlModule.setGains(command.getGains()); command.getControlFramePoseIncludingFrame(tempPosition, tempOrientation); accelerationControlModule.setBodyFixedControlFrame(tempPosition, tempOrientation); command.getIncludingFrame(tempPosition, tempLinearVelocity, feedForwardLinearAcceleration); yoDesiredPosition.setAndMatchFrame(tempPosition); yoDesiredLinearVelocity.setAndMatchFrame(tempLinearVelocity); yoFeedForwardLinearAcceleration.setAndMatchFrame(feedForwardLinearAcceleration); command.getIncludingFrame(tempOrientation, tempAngularVelocity, feedForwardAngularAcceleration); yoDesiredOrientation.setAndMatchFrame(tempOrientation); yoDesiredAngularVelocity.setAndMatchFrame(tempAngularVelocity); yoFeedForwardAngularAcceleration.setAndMatchFrame(feedForwardAngularAcceleration); }