private double getPelvisToFoot(HumanoidFloatingRootJointRobot robot) { List<GroundContactPoint> contactPoints = robot.getFootGroundContactPoints(RobotSide.LEFT); double height = Double.POSITIVE_INFINITY; for(GroundContactPoint gc : contactPoints) { if(gc.getPositionPoint().getZ() < height) { height = gc.getPositionPoint().getZ(); } } return offset.getZ() - height; }
public void setRobotsExternalForcesToMatchOtherRobot(FloatingRootJointRobot otherRobot) { ArrayList<GroundContactPoint> otherGroundContactPoints = otherRobot.getAllGroundContactPoints(); ArrayList<GroundContactPoint> groundContactPoints = robot.getAllGroundContactPoints(); for (int i = 0; i < otherGroundContactPoints.size(); i++) { GroundContactPoint otherGroundContactPoint = otherGroundContactPoints.get(i); GroundContactPoint groundContactPoint = groundContactPoints.get(i); Vector3D force = new Vector3D(); otherGroundContactPoint.getForce(force); groundContactPoint.setForce(force); Vector3D moment = new Vector3D(); otherGroundContactPoint.getMoment(moment); groundContactPoint.setMoment(moment); boolean inContact = otherGroundContactPoint.isInContact(); groundContactPoint.setIsInContact(inContact); } }
@Test// timeout = 30000 public void testDoGroundContact() { YoVariableRegistry registry = new YoVariableRegistry("CollisionGroundContactModelTest"); ArrayList<GroundContactPoint> gcPoints = new ArrayList<>(); GroundContactPoint gc = new GroundContactPoint("groundContactPoint", registry); gc.setPosition(new Point3D(0.852, 0.116, 0.099)); gcPoints.add(gc); CollisionGroundContactModel groundContactModel = new CollisionGroundContactModel(gcPoints, registry); groundContactModel.setGroundProfile3D(new RollingGroundProfile()); groundContactModel.doGroundContact(); Assert.assertTrue(gc.isInContact()); gc.setPosition(new Point3D(0.852, 0.116, 0.15)); groundContactModel.doGroundContact(); Assert.assertFalse(gc.isInContact()); } }
GroundContactPointRobotSensor(GroundContactPoint groundContactPoint) { this.groundContactPoint = groundContactPoint; position = groundContactPoint.getYoPosition(); force = groundContactPoint.getYoForce(); }
public void addYoGraphicForceVectorsToGroundContactPoints(int groupIdentifier, double forceVectorScale, AppearanceDefinition appearance, YoGraphicsListRegistry yoGraphicsListRegistry) { if (yoGraphicsListRegistry == null) return; GroundContactPointGroup groundContactPointGroup = nullJoint.getGroundContactPointGroup(groupIdentifier); System.out.println("GroundContactPointGroup" + groundContactPointGroup.getGroundContactPoints()); ArrayList<GroundContactPoint> groundContactPoints = groundContactPointGroup.getGroundContactPoints(); for (GroundContactPoint groundContactPoint : groundContactPoints) { YoGraphicVector yoGraphicVector = new YoGraphicVector(groundContactPoint.getName(), groundContactPoint.getYoPosition(), groundContactPoint.getYoForce(), forceVectorScale, appearance); yoGraphicsListRegistry.registerYoGraphic("ContactableToroidRobot", yoGraphicVector); } }
GroundContactPoint groundContactPoint = new GroundContactPoint("testPoint", registry); GroundContactPointsHolder pointsHolder = createGroundContactPointsHolder(groundContactPoint); groundContactPoint.setPosition(position); groundContactPoint.setVelocity(velocity); groundContactPoint.getForce(force); groundContactPoint.getTouchdownLocation(touchdownPosition); groundContactPoint.getForce(forceWithNormalsDisabled); groundContactPoint.setPosition(position); groundContactPoint.setVelocity(velocity); assertTrue(groundContactPoint.isInContact()); groundContactPoint.getForce(force); assertTrue(groundContactPoint.isInContact()); groundContactPoint.getForce(forceWithNormalsDisabled); groundContactPoint.getTouchdownLocation(touchdownTest); groundContactPoint.setPosition(position); groundContactPoint.setVelocity(velocity); assertFalse(groundContactPoint.isInContact()); groundContactPoint.getForce(force);
YoVariableRegistry registryOnSlope = new YoVariableRegistry("TestRegistryOnFlat"); GroundContactPoint groundContactPointOnFlat = new GroundContactPoint("testPointOnFlat", registryOnFlat); GroundContactPointsHolder pointsHolderOnFlat = createGroundContactPointsHolder(groundContactPointOnFlat); GroundContactPoint groundContactPointOnSlope = new GroundContactPoint("testPointOnSlope", registryOnSlope); GroundContactPointsHolder pointsHolderOnSlope = createGroundContactPointsHolder(groundContactPointOnSlope); Vector3D queryVelocityOnFlat = EuclidCoreRandomTools.nextVector3DWithFixedLength(random, maxAbsoluteVelocity); groundContactPointOnFlat.setPosition(queryPointOnFlat); groundContactPointOnFlat.setVelocity(queryVelocityOnFlat); groundContactModelOnFlat.doGroundContact(); Vector3D forceOnFlat = new Vector3D(); groundContactPointOnFlat.getForce(forceOnFlat); transform3D.transform(queryVelocityOnSlope); groundContactPointOnSlope.setPosition(queryPointOnSlope); groundContactPointOnSlope.setVelocity(queryVelocityOnSlope); groundContactModelOnSlope.doGroundContact(); groundContactPointOnSlope.getForce(forceOnSlope); assertTrue(groundContactPointOnFlat.isInContact() == groundContactPointOnSlope.isInContact()); assertTrue(groundContactPointOnFlat.isSlipping() == groundContactPointOnSlope.isSlipping());
private boolean footTouchedDown() { for (GroundContactPoint groundContactPoint : groundContactPoints) { if (groundContactPoint.isInContact()) return true; } return false; }
GroundContactPoint contactPoint = new GroundContactPoint("contact_" + name + "_" + joint.getName() + "_" + j, robot.getRobotsYoVariableRegistry()); joint.addGroundContactPoint(groundIdentifier, contactPoint); allGroundContactPoints.get(i).add(contactPoint); YoGraphicPosition yoGraphicPosition = new YoGraphicPosition(name + "Point" + joint.getName() + j, contactPoint.getYoPosition(), 0.02, YoAppearance.Green()); YoGraphicVector yoGraphicVector = new YoGraphicVector(name + "Force" +joint.getName() + j, contactPoint.getYoPosition(), contactPoint.getYoForce(), forceVectorScale, YoAppearance.Green()); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicPosition); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicVector);
private void createContactPoints(Robot floatingRobot) { GroundContactPoint contactPoint1 = new GroundContactPoint("contactPoint1", new Vector3D(0.0, 0.0, 0.0), floatingRobot); verticalJoint.addGroundContactPoint(1, contactPoint1); GroundContactPoint contactPoint2 = new GroundContactPoint("contactPoint2", new Vector3D(-0.002, 0.0, 0.0), floatingRobot); verticalJoint.addGroundContactPoint(1, contactPoint2); GroundContactPoint contactPoint3 = new GroundContactPoint("contactPoint3", new Vector3D(0.002, 0.0, 0.0), floatingRobot); verticalJoint.addGroundContactPoint(1, contactPoint3); ContactController contactController = new ContactController(); contactController.setContactParameters(10000.0, 1000.0, 0.5, 0.3); contactController.addContactPoints(robots[0].getAllGroundContactPoints()); ArrayList<Contactable> robotList = new ArrayList<Contactable>(); robotList.add((Contactable) robots[1]); contactController.addContactables(robotList); robots[1].setController(contactController); }
private void applyTranslationalSlip(double percentOfDelta) { FrameVector3D slipDelta = new FrameVector3D(slipAmount); slipDelta.scale(percentOfDelta); slipAmount.sub(slipDelta); Point3D touchdownLocation = new Point3D(); for (int i = 0; i < groundContactPointsToSlip.size(); i++) { GroundContactPoint groundContactPointToSlip = groundContactPointsToSlip.get(i); boolean touchedDown = (groundContactPointToSlip.isInContact()); if (touchedDown) { groundContactPointToSlip.getTouchdownLocation(touchdownLocation); touchdownLocation.add(slipDelta); groundContactPointToSlip.setTouchdownLocation(touchdownLocation); } } }
Robot robot = new Robot("testRobot"); GroundContactPoint point0 = new GroundContactPoint("point0", new Vector3D(), robot.getRobotsYoVariableRegistry()); GroundContactPoint point1 = new GroundContactPoint("point1", new Vector3D(), robot.getRobotsYoVariableRegistry()); point0.setForce(new Vector3D(0.0, 0.0, 1.0)); point1.setForce(new Vector3D(0.0, 0.0, 0.0)); point0.getYoPosition().set(new Point3D(1.0, 1.0, 0.0)); point1.getYoPosition().set(new Point3D(-1.0, 0.0, 0.0)); point0.setForce(new Vector3D(-1.0, 1.0, 0.0)); point1.setForce(new Vector3D(-1.0, 1.0, 0.0)); point0.getYoPosition().set(new Point3D(0.0, 0.0, 1.0)); point1.getYoPosition().set(new Point3D(-2.0, -2.0, 1.0));
GroundContactPoint groundContactPoint = new GroundContactPoint("testPoint", registry); GroundContactPointsHolder pointsHolder = createGroundContactPointsHolder(groundContactPoint); Vector3D velocity = new Vector3D(0.0, 0.0, 0.0); groundContactPoint.setPosition(position); groundContactPoint.setVelocity(velocity); groundContactPoint.getForce(force);
testPoint.set(groundContactPoint.getYoPosition()); pointOnPlane.changeFrame(WORLD_FRAME); groundContactPoint.getVelocity(gcVelocity); groundContactPoint.getForce(gcForce); groundContactPoint.setForce(gcForce);
private void changeAppendageGroundContactPointsToNewOffsets(HumanoidFloatingRootJointRobot robot, ArrayList<Point2D> newContactPoints, String jointName, RobotSide robotSide) { double time = robot.getTime(); System.out.println("Changing contact points at time " + time); int pointIndex = 0; ArrayList<GroundContactPoint> allGroundContactPoints = robot.getAllGroundContactPoints(); for (GroundContactPoint point : allGroundContactPoints) { Joint parentJoint = point.getParentJoint(); if (parentJoint.getName().equals(jointName)) { Point2D newContactPoint = newContactPoints.get(pointIndex); point.setIsInContact(false); Vector3D offset = new Vector3D(); point.getOffset(offset); // System.out.println("originalOffset = " + offset); offset.setX(newContactPoint.getX()); offset.setY(newContactPoint.getY()); // System.out.println("newOffset = " + offset); point.setOffsetJoint(offset); pointIndex++; } } if (footContactsInAnkleFrame != null) { footContactsInAnkleFrame.set(robotSide, newContactPoints); } }
if (gc.isInContact()) gc.getForce(force); temp_rotation.transform(force); if (gc.isInContact()) gc.getForce(force); temp_rotation.transform(force); if (gc.isInContact()) gc.getForce(force); temp_rotation.transform(force); if (gc.isInContact()) gc.getForce(force);
private Point3D computeTouchdownCoM() { int touchdownCount = 0; Point3D touchdownCoM = new Point3D(); Point3D touchdownLocation = new Point3D(); for (int i = 0; i < groundContactPointsToSlip.size(); i++) { GroundContactPoint groundContactPointToSlip = groundContactPointsToSlip.get(i); boolean touchedDown = (groundContactPointToSlip.isInContact()); if (touchedDown) { groundContactPointToSlip.getTouchdownLocation(touchdownLocation); touchdownCoM.add(touchdownLocation); touchdownCount++; } } touchdownCoM.scale(1.0/touchdownCount); return touchdownCoM; }
public SDFQuadrupedPerfectSimulatedSensor(FloatingRootJointRobot sdfRobot, FullQuadrupedRobotModel fullRobotModel, CommonQuadrupedReferenceFrames referenceFrames) { super(sdfRobot, fullRobotModel, referenceFrames); sensorOneDoFJoints = fullRobotModel.getOneDoFJoints(); //FootSwitches ArrayList<GroundContactPoint> groundContactPoints = sdfRobot.getAllGroundContactPoints(); for(RobotQuadrant quadrant : RobotQuadrant.values) { String prefix = quadrant.getCamelCaseNameForStartOfExpression(); OneDoFJoint jointBeforeFoot = fullRobotModel.getOneDoFJointBeforeFoot(quadrant); for(GroundContactPoint groundContactPoint : groundContactPoints) { if(groundContactPoint.getParentJoint().getName().equals(jointBeforeFoot.getName())) { footSwitches.set(quadrant, new SimulatedContactBasedFootSwitch(prefix + groundContactPoint.getName(), groundContactPoint, super.getYoVariableRegistry())); } } } enableDrives = new BooleanYoVariable("enableDrives", getYoVariableRegistry()); enableDrives.set(true); }
private FramePose3D getRobotFootPose(HumanoidFloatingRootJointRobot robot, RobotSide robotSide) { List<GroundContactPoint> gcPoints = robot.getFootGroundContactPoints(robotSide); Joint ankleJoint = gcPoints.get(0).getParentJoint(); RigidBodyTransform ankleTransformToWorld = new RigidBodyTransform(); ankleJoint.getTransformToWorld(ankleTransformToWorld); FramePose3D ret = new FramePose3D(); ret.set(ankleTransformToWorld); return ret; }
public void addGroundContactPoint(Robot robot, String groundContactPointName) { ArrayList<GroundContactPoint> allGroundContactPoints = robot.getAllGroundContactPoints(); for (GroundContactPoint groundContactPoint : allGroundContactPoints) { if (groundContactPoint.getName().equals(groundContactPointName)) { this.addGroundContactPoint(groundContactPoint); return; } } }