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; }
private FramePose2D getRobotFootPose2d(HumanoidFloatingRootJointRobot robot, RobotSide robotSide) { List<GroundContactPoint> gcPoints = robot.getFootGroundContactPoints(robotSide); Joint ankleJoint = gcPoints.get(0).getParentJoint(); RigidBodyTransform ankleTransformToWorld = new RigidBodyTransform(); ankleJoint.getTransformToWorld(ankleTransformToWorld); FramePose2D ret = new FramePose2D(); ret.setIncludingFrame(ReferenceFrame.getWorldFrame(), ankleTransformToWorld, false); return ret; }
private FramePose2D getRobotFootPose2d(HumanoidFloatingRootJointRobot robot, RobotSide robotSide) { List<GroundContactPoint> gcPoints = robot.getFootGroundContactPoints(robotSide); Joint ankleJoint = gcPoints.get(0).getParentJoint(); RigidBodyTransform ankleTransformToWorld = new RigidBodyTransform(); ankleJoint.getTransformToWorld(ankleTransformToWorld); FramePose2D ret = new FramePose2D(); ret.setIncludingFrame(ReferenceFrame.getWorldFrame(), ankleTransformToWorld, false); return ret; }
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); } }
private void changeAppendageGroundContactPointsToNewOffsets(HumanoidFloatingRootJointRobot robot, ArrayList<? extends Point2DBasics> contactPointsInAnkleFrame, 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)) { Point2DBasics newContactPoint = contactPointsInAnkleFrame.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, contactPointsInAnkleFrame); } }
private void setNewContacts() { String footJointName = drcSimulationTestHelper.getControllerFullRobotModel().getFoot(robotSide).getParentJoint().getName(); HumanoidFloatingRootJointRobot robot = drcSimulationTestHelper.getRobot(); int pointIndex = 0; ArrayList<GroundContactPoint> allGroundContactPoints = robot.getAllGroundContactPoints(); for (GroundContactPoint point : allGroundContactPoints) { Joint parentJoint = point.getParentJoint(); if (parentJoint.getName().equals(footJointName)) { Point2D newContactPoint = newContactPoints.get(pointIndex); point.setIsInContact(false); Vector3D offset = new Vector3D(); point.getOffset(offset); offset.setX(newContactPoint.getX()); offset.setY(newContactPoint.getY()); point.setOffsetJoint(offset); pointIndex++; } } if (footContactsInAnkleFrame != null) { footContactsInAnkleFrame.set(robotSide, newContactPoints); } setNewContactPoints.set(false); }
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); }
Joint leftAnkleJoint = footGroundContactPoints.get(0).getParentJoint(); Joint rightAnkleJoint = footGroundContactPoints.get(0).getParentJoint();