public void update(ExternalForcePoint efp) { update(efp.getYoPosition(), efp.getYoForce()); }
@Override public void initialize() { robot.update(); for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { initialPositions.get(i).set(externalForcePoints.get(i).getYoPosition()); 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 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(); }
public void update(ExternalForcePoint efp) { update(efp.getYoPosition().getFrameTuple(), efp.getYoForce().getFrameTuple()); }
@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(); } }
@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(); } }
@Override public void doControl() { for (int i = 0; i < efp_offsetFromRootJoint.size(); i++) { initialPositions.get(i).setZ(desiredHeight.getDoubleValue()); ExternalForcePoint efp = externalForcePoints.get(i); proportionalTerm.set(efp.getYoPosition()); proportionalTerm.sub(initialPositions.get(i)); proportionalTerm.scale(-holdPelvisKp.getDoubleValue()); // proportionalTerm.setZ(Math.max(proportionalTerm.getZ(), 0.0)); derivativeTerm.set(efp.getYoVelocity()); derivativeTerm.scale(-holdPelvisKv.getDoubleValue()); pdControlOutput.add(proportionalTerm, derivativeTerm); efp.setForce(pdControlOutput); efp.getYoForce().getYoZ().add(robotWeight / efp_offsetFromRootJoint.size()); efp_positionViz.get(i).update(); } }
jointToAddExternalForcePoints.addExternalForcePoint(efp); efp_positionViz.add(new YoGraphicPosition(efp.getName(), efp.getYoPosition(), 0.05, YoAppearance.Red()));
jointToAddExternalForcePoints.addExternalForcePoint(efp); efp_positionViz.add(new YoGraphicPosition(efp.getName(), efp.getYoPosition(), 0.05, YoAppearance.Red()));
jointToAddExternalForcePoints.addExternalForcePoint(efp); efp_positionViz.add(new YoGraphicPosition(efp.getName(), efp.getYoPosition(), 0.05, YoAppearance.Red()));
public PushRobotController(FloatingRootJointRobot pushableRobot, String jointNameToApplyForce, Vector3d forcePointOffset) { yoTime = pushableRobot.getYoTime(); registry = new YoVariableRegistry(jointNameToApplyForce + "_" + getClass().getSimpleName()); forcePoint = new ExternalForcePoint(jointNameToApplyForce + "_externalForcePoint", forcePointOffset, pushableRobot); pushDuration = new DoubleYoVariable(jointNameToApplyForce + "_pushDuration", registry); pushForceMagnitude = new DoubleYoVariable(jointNameToApplyForce + "_pushMagnitude", registry); pushDirection = new YoFrameVector(jointNameToApplyForce + "_pushDirection", worldFrame, registry); pushForce = new YoFrameVector(jointNameToApplyForce + "_pushForce", worldFrame, registry); pushTimeSwitch = new DoubleYoVariable(jointNameToApplyForce + "_pushTimeSwitch", registry); pushNumber = new IntegerYoVariable(jointNameToApplyForce + "_pushNumber", registry); isBeingPushed = new BooleanYoVariable(jointNameToApplyForce + "_isBeingPushed", registry); pushDelay = new DoubleYoVariable(jointNameToApplyForce + "_pushDelay", registry); pushableRobot.getJoint(jointNameToApplyForce).addExternalForcePoint(forcePoint); pushableRobot.setController(this); pushTimeSwitch.set(Double.NEGATIVE_INFINITY); pushForceMagnitude.set(0.0); forceVisualizer = new YoGraphicVector(jointNameToApplyForce + "_pushForce", forcePoint.getYoPosition(), forcePoint.getYoForce(), 0.005, YoAppearance.DarkBlue()); }
public PushRobotController(FloatingRootJointRobot pushableRobot, String jointNameToApplyForce, Vector3D forcePointOffset, double visualScale) { yoTime = pushableRobot.getYoTime(); registry = new YoVariableRegistry(jointNameToApplyForce + "_" + getClass().getSimpleName()); forcePoint = new ExternalForcePoint(jointNameToApplyForce + "_externalForcePoint", forcePointOffset, pushableRobot); pushDuration = new YoDouble(jointNameToApplyForce + "_pushDuration", registry); pushForceMagnitude = new YoDouble(jointNameToApplyForce + "_pushMagnitude", registry); pushDirection = new YoFrameVector3D(jointNameToApplyForce + "_pushDirection", worldFrame, registry); pushForce = new YoFrameVector3D(jointNameToApplyForce + "_pushForce", worldFrame, registry); pushTimeSwitch = new YoDouble(jointNameToApplyForce + "_pushTimeSwitch", registry); pushNumber = new YoInteger(jointNameToApplyForce + "_pushNumber", registry); isBeingPushed = new YoBoolean(jointNameToApplyForce + "_isBeingPushed", registry); pushDelay = new YoDouble(jointNameToApplyForce + "_pushDelay", registry); pushableRobot.getJoint(jointNameToApplyForce).addExternalForcePoint(forcePoint); pushableRobot.setController(this); pushTimeSwitch.set(Double.NEGATIVE_INFINITY); pushForceMagnitude.set(0.0); forceVisualizer = new YoGraphicVector(jointNameToApplyForce + "_pushForce", forcePoint.getYoPosition(), forcePoint.getYoForce(), visualScale, YoAppearance.DarkBlue()); }
private void getExternalWrenchesFromSCS() { calculator.setExternalWrenchesToZero(); for (ExternalForcePoint efp : robot.getAllGroundContactPoints()) { String parentJointName = efp.getParentJoint().getName(); RigidBodyBasics body = nameToJointMap.get(parentJointName).getSuccessor(); FrameVector3DReadOnly moment = efp.getYoMoment(); FrameVector3DReadOnly force = efp.getYoForce(); FramePoint3D pointOfApplication = new FramePoint3D(efp.getYoPosition()); pointOfApplication.changeFrame(body.getBodyFixedFrame()); SpatialVector vector6D = new SpatialVector(moment, force); vector6D.changeFrame(body.getBodyFixedFrame()); Wrench externalWrench = new Wrench(body.getBodyFixedFrame(), body.getBodyFixedFrame()); externalWrench.set(vector6D.getAngularPart(), vector6D.getLinearPart(), pointOfApplication); calculator.getExternalWrench(body).add(externalWrench); } }
graphics3DNodeType = Graphics3DNodeType.GROUND; simpleExternalForcePoint = new ExternalForcePoint("simpleExternalForcePoint", dummyRegistry); yoGraphic = new YoGraphicVector("simpleDynamicGraphicObject", simpleExternalForcePoint.getYoPosition(), simpleExternalForcePoint.getYoForce(), 1.0 / 50.0); exitActionListenerHasBeenNotified = new YoBoolean("exitActionListenerHasBeenNotified", dummyRegistry);
forceVisualizer = new YoGraphicVector("contactForceVisualizer" + suffix, forcePoint.getYoPosition(), contactForce, 0.05, forceAppearance);