private void addForcePoint() { externalForcePoint = new ExternalForcePoint("handForcePoint", scsRobotArm.getLink("hand").getComOffset(), scsRobotArm); scsRobotArm.getLink("hand").getParentJoint().addExternalForcePoint(externalForcePoint); }
@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 update(ExternalForcePoint efp) { update(efp.getYoPosition(), efp.getYoForce()); }
public void setRobotRootJointExternalForcesRandomly(Random random, double maxRootJointForceAndTorque) { rootJointExternalForcePoint.setForce(RandomGeometry.nextVector3D(random, maxRootJointForceAndTorque)); rootJointExternalForcePoint.setMoment(RandomGeometry.nextVector3D(random, maxRootJointForceAndTorque)); }
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()); }
ExternalForcePoint efp = new ExternalForcePoint("efp_" + linkName + "_" + String.valueOf(i) + "_", efp_offsetFromRootJoint.get(i), robot.getRobotsYoVariableRegistry()); externalForcePoints.add(efp); jointToAddExternalForcePoints.addExternalForcePoint(efp); efp_positionViz.add(new YoGraphicPosition(efp.getName(), efp.getYoPosition(), 0.05, YoAppearance.Red()));
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); } }
ExternalForcePoint externalForcePoint = new ExternalForcePoint("efp", externalForcePointOffset, robot.getRobotsYoVariableRegistry()); root1.addExternalForcePoint(externalForcePoint); Vector3D force = new Vector3D(random.nextDouble(), random.nextDouble(), random.nextDouble()); externalForcePoint.setForce(force);
ExternalForcePoint externalForcePoint = new ExternalForcePoint("rootExternalForcePoint", externalForcePointOffset, robot.getRobotsYoVariableRegistry()); rootJoint.addExternalForcePoint(externalForcePoint); externalForcePoint.setForce(random.nextDouble(), random.nextDouble(), random.nextDouble()); externalForcePoint.getForce(externalForce); // in world frame RigidBodyTransform worldToBody = elevatorFrame.getTransformToDesiredFrame(forceApplicationFrame); worldToBody.transform(externalForce);
@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)); }
externalForcePoint.setForce(0.0, 0.0, 0.0); pointPosition.set(externalForcePoint.getX(), externalForcePoint.getY(), externalForcePoint.getZ()); externalForcePoint.setForce(0.0, 0.0, 0.0); double springForce = hoistStiffness.getDoubleValue() * delta; velocityVector.set(externalForcePoint.getXVelocity(), externalForcePoint.getYVelocity(), externalForcePoint.getZVelocity()); forceVector.normalize(); externalForcePoint.setForce(forceVector);
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)); } }
@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 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(); }
linearPidGains = new YoPIDGains(forcePoint.getName() + "_linear" + suffix, registry); linearPidGains.setKp(linearKp); linearPidGains.setKi(linearKi); linearPidGains.setPositionDeadband(linearDeadband); angularPidGains = new YoPIDGains(forcePoint.getName() + "_angular" + suffix, registry); angularPidGains.setKp(angularKp); angularPidGains.setKi(angularKi); forceVisualizer = new YoGraphicVector("contactForceVisualizer" + suffix, forcePoint.getYoPosition(), contactForce, 0.05, forceAppearance);
floatingJoint.setLink(link); ExternalForcePoint externalForcePoint = new ExternalForcePoint("externalForcePoint", new Vector3D(), robot); floatingJoint.addExternalForcePoint(externalForcePoint); Vector3D impulseInWorldToPack = new Vector3D(); externalForcePoint.resolveCollision(velocityOfOtherObjectInWorld, collisionNormalInWorld, epsilon, mu, impulseInWorldToPack); initialVelocity = new Vector3D(0.1, 0.07, -1.0); externalForcePoint.setOffsetJoint(offsetFromCenterOfMass); externalForcePoint.getVelocity(initialVelocityAtImpactPoint); impulseInWorldToPack = new Vector3D(); externalForcePoint.resolveCollision(velocityOfOtherObjectInWorld, collisionNormalInWorld, epsilon, mu, impulseInWorldToPack); externalForcePoint.getVelocity(finalVelocityAtImpactPoint);
@Override public boolean isInContact() { double force = forcePoint.getYoForce().length(); isInContact.set(force >= contactForceThreshold.getDoubleValue()); return isInContact.getBooleanValue(); }
private Vector3D quadraticCutForceModel(ExternalForcePoint forcePoint) tangentVector.set(forcePoint.getVelocityVector()); tangentionalVelocity.set(forcePoint.getVelocityVector()); if(tangentVector.length() != 0.0 && forcePoint.getPositionPoint().getZ() > 0.75 && forcePoint.getPositionPoint().getX() > 0.5) tangentVector.scale(quadraticForceCoeff.getDoubleValue() * Math.pow(forcePoint.getVelocityVector().length(), 2)); climbingForceVector.scale(tangentionalVelocity.length() * 100.0);
private Vector3d exponentialCutForceModel(ExternalForcePoint forcePoint) { tangentVector.set(forcePoint.getVelocityVector()); tangentionalVelocity.set(forcePoint.getVelocityVector()); if(tangentVector.length() != 0.0) { tangentionalVelocity.setX(0.0); tangentVector.setX(0.0); tangentVector.normalize(); tangentVector.scale(-1.0); tangentVector.scale(90.0 * (Math.exp(1.0 *tangentionalVelocity.length()) - 1.0)); efpHandControlFrameVelocity.set(tangentionalVelocity.length()); return tangentVector; } else { return tangentVector; } } }
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()); }