private boolean footTouchedDown() { for (GroundContactPoint groundContactPoint : groundContactPoints) { if (groundContactPoint.isInContact()) return true; } return false; }
private boolean footLiftedUp() { for (GroundContactPoint groundContactPoint : groundContactPoints) { if (groundContactPoint.isInContact()) return false; } return true; }
private boolean footTouchedDown(RobotSide robotSide) { for (GroundContactPoint groundContactPoint : groundContactPointsMap.get(robotSide)) { if (groundContactPoint.isInContact()) return true; } return false; }
private boolean footLiftedUp(RobotSide robotSide) { for (GroundContactPoint groundContactPoint : groundContactPointsMap.get(robotSide)) { if (groundContactPoint.isInContact()) return false; } return true; } }
private boolean footTouchedDown(RobotSide robotSide) { for (GroundContactPoint groundContactPoint : groundContactPointsMap.get(robotSide)) { if (groundContactPoint.isInContact()) return true; } return false; } }
private boolean footTouchedDown(RobotSide robotSide) { for (GroundContactPoint groundContactPoint : groundContactPointsMap.get(robotSide)) { if (groundContactPoint.isInContact()) return true; } return false; } }
private boolean footTouchedDown(RobotSide robotSide) { for (GroundContactPoint groundContactPoint : groundContactPointsMap.get(robotSide)) { if (groundContactPoint.isInContact()) return true; } return false; } }
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 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); } }
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); } } }
@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()); } }
if (gc.isInContact()) if (gc.isInContact()) if (gc.isInContact()) if (gc.isInContact())
private void applyRotationalSlip(double percentOfDelta) { FrameQuaternion identity = new FrameQuaternion(ReferenceFrame.getWorldFrame()); FrameQuaternion desired = slipRotation.getFrameOrientationCopy(); FrameQuaternion delta = new FrameQuaternion(); delta.interpolate(identity, desired, percentOfDelta); desired.interpolate(identity, desired, 1.0-percentOfDelta); slipRotation.set(desired); Point3D touchdownCoM = computeTouchdownCoM(); RotationMatrix deltaRotation = new RotationMatrix(delta); 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.sub(touchdownCoM); deltaRotation.transform(touchdownLocation); touchdownLocation.add(touchdownCoM); groundContactPointToSlip.setTouchdownLocation(touchdownLocation); } } }
assertTrue(groundContactPoint.isInContact()); groundContactPoint.getForce(force); assertTrue(groundContactPoint.isInContact()); groundContactPoint.getForce(forceWithNormalsDisabled); assertFalse(groundContactPoint.isInContact()); groundContactPoint.getForce(force);
assertTrue(groundContactPointOnFlat.isInContact() == groundContactPointOnSlope.isInContact()); assertTrue(groundContactPointOnFlat.isSlipping() == groundContactPointOnSlope.isSlipping());