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); }
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 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); }
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); } }
joint.addGroundContactPoint(trackGroundContactPoint); trackPoints[i][j] = trackGroundContactPoint;
ballRobot.getRootJoints().get(0).addGroundContactPoint(gc);