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); }