public void setRobotRootJointExternalForcesRandomly(Random random, double maxRootJointForceAndTorque) { rootJointExternalForcePoint.setForce(RandomGeometry.nextVector3D(random, maxRootJointForceAndTorque)); rootJointExternalForcePoint.setMoment(RandomGeometry.nextVector3D(random, maxRootJointForceAndTorque)); }
public void setInitialForce(Vector3D initialForce, Vector3D initialTorque) { forcePoint.setForce(initialForce); forcePoint.setMoment(initialTorque); this.initialForce.set(initialForce); this.initialTorque.set(initialTorque); hasInitialForce = true; }
@Override public void initialize() { if (!hasInitialForce) forcePoint.setForce(0.0, 0.0, 0.0); pidControllerAngularX.resetIntegrator(); pidControllerAngularY.resetIntegrator(); pidControllerAngularZ.resetIntegrator(); pidControllerLinearX.resetIntegrator(); pidControllerLinearY.resetIntegrator(); pidControllerLinearZ.resetIntegrator(); }
@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(); } }
@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(); } }
public void setRobotExternalForcesRandomly(Random random, double maxGroundContactPointForce, double maxFootExternalForce, double maxFootExternalTorque) { for (RobotSide robotSide : RobotSide.values) { List<GroundContactPoint> footGroundContactPoints = robot.getFootGroundContactPoints(robotSide); for (GroundContactPoint groundContactPoint : footGroundContactPoints) { groundContactPoint.setForce(RandomGeometry.nextVector3D(random, maxGroundContactPointForce)); } ExternalForcePoint footExternalForcePoint = feetExternalForcePoints.get(robotSide); footExternalForcePoint.setForce(RandomGeometry.nextVector3D(random, maxFootExternalForce)); footExternalForcePoint.setMoment(RandomGeometry.nextVector3D(random, maxFootExternalTorque)); } }
@Override public void doControl() { wristJoint.getTransformToWorld(transform); wristJointPose.setPose(transform); yoWristJointPose.set(wristJointPose); wristJoint.getTransformToWorld(transform); transform.transform(wristToHandControlFrame, tangentVector); handControlFramePose.setPose(transform); handControlFramePose.translate(tangentVector); yoHandControlFramePose.set(handControlFramePose); handControlFramePose.getPosition(handControlFramePositionInWorld); efpHandControlFrame.setPosition(handControlFramePositionInWorld); // efpGravity.setForce(0.0, 0.0, -gGRAVITY * MASSDRILL); // efpGravity.setForce(0.0, 0.0, 0.0); // efpWrist.setForce(exponentialCutForceModel(efpHandControlFrame)); efpWrist.setForce(quadraticCutForceModel(efpHandControlFrame)); }
@Override public void doControl() { wristJoint.getTransformToWorld(transform); wristJointPose.set(transform); yoWristJointPose.set(wristJointPose); wristJoint.getTransformToWorld(transform); transform.transform(wristToHandControlFrame, tangentVector); handControlFramePose.set(transform); handControlFramePose.prependTranslation(tangentVector); yoHandControlFramePose.set(handControlFramePose); handControlFramePositionInWorld.set(handControlFramePose.getPosition()); efpHandControlFrame.setPosition(handControlFramePositionInWorld); // efpGravity.setForce(0.0, 0.0, -gGRAVITY * MASSDRILL); // efpGravity.setForce(0.0, 0.0, 0.0); // efpWrist.setForce(exponentialCutForceModel(efpHandControlFrame)); efpWrist.setForce(quadraticCutForceModel(efpHandControlFrame)); }
@Override public void doControl() { wristJoint.getTransformToWorld(transform); wristJointPose.setPose(transform); yoWristJointPose.set(wristJointPose); wristJoint.getTransformToWorld(transform); transform.transform(wristToHandControlFrame, tangentVector); handControlFramePose.setPose(transform); handControlFramePose.translate(tangentVector); yoHandControlFramePose.set(handControlFramePose); handControlFramePose.getPosition(handControlFramePositionInWorld); efpHandControlFrame.setPosition(handControlFramePositionInWorld); // efpGravity.setForce(0.0, 0.0, -gGRAVITY * MASSDRILL); // efpGravity.setForce(0.0, 0.0, 0.0); // efpWrist.setForce(exponentialCutForceModel(efpHandControlFrame)); efpWrist.setForce(quadraticCutForceModel(efpHandControlFrame)); }
@Override public void doControl() { if (pushCondition != null) { if (pushCondition.checkCondition()) { pushTimeSwitch.set(yoTime.getDoubleValue() + pushDelay.getDoubleValue()); pushCondition = null; } } if (yoTime.getDoubleValue() <= pushTimeSwitch.getDoubleValue() + pushDuration.getDoubleValue() && yoTime.getDoubleValue() >= pushTimeSwitch.getDoubleValue()) { isBeingPushed.set(true); pushForce.get(forceVector); pushNumber.decrement(); } else { isBeingPushed.set(false); forceVector.set(0.0, 0.0, 0.0); } forcePoint.setForce(forceVector); }
ExternalForcePoint externalForcePoint = new ExternalForcePoint("rootExternalForcePoint", externalForcePointOffset, robot.getRobotsYoVariableRegistry()); rootJoint.addExternalForcePoint(externalForcePoint); externalForcePoint.setForce(random.nextDouble(), random.nextDouble(), random.nextDouble());
root1.addExternalForcePoint(externalForcePoint); Vector3D force = new Vector3D(random.nextDouble(), random.nextDouble(), random.nextDouble()); externalForcePoint.setForce(force);
@Override public void doControl() { if (pushCondition != null) { if (pushCondition.testCondition(yoTime.getDoubleValue())) { pushTimeSwitch.set(yoTime.getDoubleValue() + pushDelay.getDoubleValue()); pushCondition = null; } } if (yoTime.getDoubleValue() <= pushTimeSwitch.getDoubleValue() + pushDuration.getDoubleValue() && yoTime.getDoubleValue() >= pushTimeSwitch.getDoubleValue()) { isBeingPushed.set(true); forceVector.set(pushForce); pushNumber.decrement(); } else { isBeingPushed.set(false); forceVector.set(0.0, 0.0, 0.0); } forcePoint.setForce(forceVector); }
public void setRobotTorquesToMatchFullRobotModel() { FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); ReferenceFrame bodyFixedFrame = fullRobotModel.getPelvis().getBodyFixedFrame(); Wrench rootJointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); rootJointWrench.setIncludingFrame(rootJoint.getJointWrench()); rootJointWrench.changeFrame(bodyFixedFrame); FrameVector3D rootJointForce = new FrameVector3D(rootJointWrench.getLinearPart()); FrameVector3D rootJointTorque = new FrameVector3D(rootJointWrench.getAngularPart()); rootJointForce.changeFrame(ReferenceFrame.getWorldFrame()); rootJointTorque.changeFrame(ReferenceFrame.getWorldFrame()); computedRootJointForces.set(rootJointForce); computedRootJointTorques.set(rootJointTorque); rootJointExternalForcePoint.setForce(new Vector3D(rootJointForce)); rootJointExternalForcePoint.setMoment(new Vector3D(rootJointTorque)); FramePoint3D rootJointPosition = new FramePoint3D(bodyFixedFrame); rootJointPosition.changeFrame(ReferenceFrame.getWorldFrame()); rootJointExternalForcePoint.setOffsetWorld(rootJointPosition); ArrayList<OneDegreeOfFreedomJoint> oneDegreeOfFreedomJoints = new ArrayList<OneDegreeOfFreedomJoint>(); robot.getAllOneDegreeOfFreedomJoints(oneDegreeOfFreedomJoints); for (OneDegreeOfFreedomJoint oneDegreeOfFreedomJoint : oneDegreeOfFreedomJoints) { OneDoFJointBasics oneDoFJoint = fullRobotModel.getOneDoFJointByName(oneDegreeOfFreedomJoint.getName()); double inverseDynamicsTorque = oneDoFJoint.getTau(); oneDegreeOfFreedomJoint.setTau(inverseDynamicsTorque); YoDouble computedJointTorque = computedJointTorques.get(oneDoFJoint); computedJointTorque.set(inverseDynamicsTorque); } }
public void setRobotsExternalForcesToMatchFullRobotModel(InverseDynamicsCalculator inverseDynamicsCalculator) { for (RobotSide robotSide : RobotSide.values) { RigidBodyBasics foot = fullRobotModel.getFoot(robotSide); Wrench wrench = new Wrench(inverseDynamicsCalculator.getExternalWrench(foot)); ReferenceFrame bodyFixedFrame = foot.getBodyFixedFrame(); FramePoint3D pointOfWrenchApplication = new FramePoint3D(bodyFixedFrame); pointOfWrenchApplication.changeFrame(ReferenceFrame.getWorldFrame()); ExternalForcePoint footExternalForcePoint = feetExternalForcePoints.get(robotSide); footExternalForcePoint.setOffsetWorld(pointOfWrenchApplication); FrameVector3D wrenchForce = new FrameVector3D(wrench.getLinearPart()); wrenchForce.changeFrame(ReferenceFrame.getWorldFrame()); FrameVector3D wrenchTorque = new FrameVector3D(wrench.getAngularPart()); wrenchTorque.changeFrame(ReferenceFrame.getWorldFrame()); footExternalForcePoint.setForce(new Vector3D(wrenchForce)); footExternalForcePoint.setMoment(new Vector3D(wrenchTorque)); } }
double mass = this.computeCenterOfMass(comPoint); externalForceVector.scale(-mass); externalForcePoint.setForce(externalForceVector);
contactTorque.setZ(torqueZ); forcePoint.setForce(linearForceX, linearForceY, linearForceZ); forcePoint.setMoment(torqueX, torqueY, torqueZ);