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; } } }
public DRCSimulationVisualizer(Robot robot, YoGraphicsListRegistry yoGraphicsListRegistry) { this.robot = robot; YoGraphicsList yoGraphicsList = new YoGraphicsList("Simulation Viz"); ArrayList<GroundContactPoint> groundContactPoints = robot.getAllGroundContactPoints(); AppearanceDefinition appearance = YoAppearance.Red(); // BlackMetalMaterial(); for (GroundContactPoint groundContactPoint : groundContactPoints) { double scaleFactor = 0.0015; YoGraphicVector dynamicGraphicVector = new YoGraphicVector(groundContactPoint.getName(), groundContactPoint.getYoPosition(), groundContactPoint.getYoForce(), scaleFactor, appearance); yoGraphicsList.add(dynamicGraphicVector); } if (yoGraphicsListRegistry != null) yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); robot.setController(this, 10); }
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 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); } }
GroundContactPoint groundContactPoint = robot.getAllGroundContactPoints().get(0); YoFramePoint3D yoPosition = groundContactPoint.getYoPosition(); xPosition = dataBuffer.getEntry(yoPosition.getYoX()).getData();