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); }
public DRCDrillEnvironment() { double forceVectorScale = 1.0 / 500.0; combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName()); combinedTerrainObject.addTerrainObject(setUpGround("Ground", tableCenter, 0.1)); RigidBodyTransform initialDrillTransform = new RigidBodyTransform(new AxisAngle4d(), tableCenter); drillRobot = new ContactableCylinderRobot("drill", initialDrillTransform , drillRadius, drillHeight, drillMass, "models/drill.obj"); final int groundContactGroupIdentifier = 0; drillRobot.createAvailableContactPoints(groundContactGroupIdentifier, 30, forceVectorScale, true); for (int i = 0; i < 4; i++) { double angle = i * 2.0 * Math.PI / 4.0; double x = 1.5 * drillRadius * Math.cos(angle); double y = 1.5 * drillRadius * Math.sin(angle); GroundContactPoint groundContactPoint = new GroundContactPoint("gc_drill_" + i, new Vector3d(x, y, 0.0), drillRobot); drillRobot.getRootJoints().get(0).addGroundContactPoint(groundContactPoint); } GroundContactModel groundContactModel = new LinearGroundContactModel(drillRobot, groundContactGroupIdentifier,1422.0, 150.6, 50.0, 600.0, drillRobot.getRobotsYoVariableRegistry()); groundContactModel.setGroundProfile3D(combinedTerrainObject); drillRobot.setGroundContactModel(groundContactModel); robots.add(drillRobot); }
GroundContactPoint contactPoint = new GroundContactPoint("contact_" + name + "_" + joint.getName() + "_" + j, robot.getRobotsYoVariableRegistry()); joint.addGroundContactPoint(groundIdentifier, contactPoint); allGroundContactPoints.get(i).add(contactPoint);
public HandDrillManipulationEnvironment() { double forceVectorScale = 1.0 / 500.0; combinedTerrainObject = new CombinedTerrainObject3D(getClass().getSimpleName()); combinedTerrainObject.addTerrainObject(setUpGround("Ground", tableCenter, 0.1)); RigidBodyTransform initialDrillTransform = new RigidBodyTransform(new AxisAngle(), tableCenter); drillRobot = new ContactableCylinderRobot("drill", initialDrillTransform , drillRadius, drillHeight, drillMass, "models/drill.obj"); final int groundContactGroupIdentifier = 0; drillRobot.createAvailableContactPoints(groundContactGroupIdentifier, 30, forceVectorScale, true); for (int i = 0; i < 4; i++) { double angle = i * 2.0 * Math.PI / 4.0; double x = 1.5 * drillRadius * Math.cos(angle); double y = 1.5 * drillRadius * Math.sin(angle); GroundContactPoint groundContactPoint = new GroundContactPoint("gc_drill_" + i, new Vector3D(x, y, 0.0), drillRobot); drillRobot.getRootJoints().get(0).addGroundContactPoint(groundContactPoint); } GroundContactModel groundContactModel = new LinearGroundContactModel(drillRobot, groundContactGroupIdentifier,1422.0, 150.6, 50.0, 600.0, drillRobot.getRobotsYoVariableRegistry()); groundContactModel.setGroundProfile3D(combinedTerrainObject); drillRobot.setGroundContactModel(groundContactModel); robots.add(drillRobot); }
private void setUpGroundContactPoints(FrameBox3d frameBox) { String name = this.getName(); for (int i = 0; i < 8; i++) { Point3D vertex = new Point3D(); frameBox.getBox3d().getVertex(i, vertex); GroundContactPoint groundContactPoint = new GroundContactPoint("gc_" + name + i, new Vector3D(vertex), this.getRobotsYoVariableRegistry()); floatingJoint.addGroundContactPoint(groundContactPoint); } }
private void createDrill() { Vector3D tableCenter = new Vector3D(-0.851, -6.833, 1.118); double drillHeight = 0.3; double drillRadius = 0.03; double drillMass = 1.5; double forceVectorScale = 1.0 / 500.0; RigidBodyTransform initialDrillTransform = new RigidBodyTransform(new AxisAngle(), tableCenter); ContactableCylinderRobot drillRobot = new ContactableCylinderRobot("drill", initialDrillTransform, drillRadius, drillHeight, drillMass, "models/drill.obj"); final int groundContactGroupIdentifier = 0; drillRobot.createAvailableContactPoints(groundContactGroupIdentifier, 30, forceVectorScale, true); for (int i = 0; i < 4; i++) { double angle = i * 2.0 * Math.PI / 4.0; double x = 1.5 * drillRadius * Math.cos(angle); double y = 1.5 * drillRadius * Math.sin(angle); GroundContactPoint groundContactPoint = new GroundContactPoint("gc_drill_" + i, new Vector3D(x, y, 0.0), drillRobot); drillRobot.getRootJoints().get(0).addGroundContactPoint(groundContactPoint); } GroundContactModel groundContactModel = new LinearGroundContactModel(drillRobot, groundContactGroupIdentifier, 1422.0, 150.6, 50.0, 600.0, drillRobot.getRobotsYoVariableRegistry()); groundContactModel.setGroundProfile3D(combinedTerrainObject); drillRobot.setGroundContactModel(groundContactModel); contactableRobots.add(drillRobot); combinedTerrainObject.addBox(-0.777, -6.916, -0.947, -6.746, 1.061, 1.08); }
private void createDrill() { Vector3d tableCenter = new Vector3d(-0.851, -6.833, 1.118); double drillHeight = 0.3; double drillRadius = 0.03; double drillMass = 1.5; double forceVectorScale = 1.0 / 500.0; RigidBodyTransform initialDrillTransform = new RigidBodyTransform(new AxisAngle4d(), tableCenter); ContactableCylinderRobot drillRobot = new ContactableCylinderRobot("drill", initialDrillTransform, drillRadius, drillHeight, drillMass, "models/drill.obj"); final int groundContactGroupIdentifier = 0; drillRobot.createAvailableContactPoints(groundContactGroupIdentifier, 30, forceVectorScale, true); for (int i = 0; i < 4; i++) { double angle = i * 2.0 * Math.PI / 4.0; double x = 1.5 * drillRadius * Math.cos(angle); double y = 1.5 * drillRadius * Math.sin(angle); GroundContactPoint groundContactPoint = new GroundContactPoint("gc_drill_" + i, new Vector3d(x, y, 0.0), drillRobot); drillRobot.getRootJoints().get(0).addGroundContactPoint(groundContactPoint); } GroundContactModel groundContactModel = new LinearGroundContactModel(drillRobot, groundContactGroupIdentifier, 1422.0, 150.6, 50.0, 600.0, drillRobot.getRobotsYoVariableRegistry()); groundContactModel.setGroundProfile3D(combinedTerrainObject); drillRobot.setGroundContactModel(groundContactModel); contactableRobots.add(drillRobot); combinedTerrainObject.addBox(-0.777, -6.916, -0.947, -6.746, 1.061, 1.08); }
@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()); } }
public void createAvailableContactPoints(int groupIdentifier, int totalContactPointsAvailable, double forceVectorScale, boolean addYoGraphicForceVectorsForceVectors) { YoGraphicsListRegistry yoGraphicsListRegistry = null; if (addYoGraphicForceVectorsForceVectors) yoGraphicsListRegistry = new YoGraphicsListRegistry(); for (int i = 0; i < totalContactPointsAvailable; i++) { GroundContactPoint contactPoint = new GroundContactPoint("contact_" + name + "_" + i, robot.getRobotsYoVariableRegistry()); getJoint().addGroundContactPoint(groupIdentifier, contactPoint); allGroundContactPoints.add(contactPoint); YoBoolean contactAvailable = new YoBoolean("contact_" + name + "_" + i + "_avail", robot.getRobotsYoVariableRegistry()); contactAvailable.set(true); contactsAvailable.add(contactAvailable); if (addYoGraphicForceVectorsForceVectors) { YoGraphicPosition yoGraphicPosition = new YoGraphicPosition(name + "Point" + i, contactPoint.getYoPosition(), 0.02, YoAppearance.Green()); YoGraphicVector yoGraphicVector = new YoGraphicVector(name + "Force" + i, contactPoint.getYoPosition(), contactPoint.getYoForce(), forceVectorScale, YoAppearance.Green()); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicPosition); yoGraphicsListRegistry.registerYoGraphic(name, yoGraphicVector); } } if (addYoGraphicForceVectorsForceVectors) { robot.addYoGraphicsListRegistry(yoGraphicsListRegistry); } }
GroundContactPoint gc = new GroundContactPoint("ballGC_" + i, new Vector3D(contactPointOffset), ballRobot); ballRobot.getRootJoints().get(0).addGroundContactPoint(gc);
GroundContactPoint groundContactPoint = new GroundContactPoint("testPoint", registry); GroundContactPointsHolder pointsHolder = createGroundContactPointsHolder(groundContactPoint);
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);
Robot robot = new Robot("testRobot"); GroundContactPoint point0 = new GroundContactPoint("point0", new Vector3D(), robot.getRobotsYoVariableRegistry()); GroundContactPoint point1 = new GroundContactPoint("point1", new Vector3D(), robot.getRobotsYoVariableRegistry());
GroundContactPoint groundContactPoint = new GroundContactPoint("testPoint", registry); GroundContactPointsHolder pointsHolder = createGroundContactPointsHolder(groundContactPoint);