public void setRobotsExternalForcesToMatchOtherRobot(FloatingRootJointRobot otherRobot) { ArrayList<GroundContactPoint> otherGroundContactPoints = otherRobot.getAllGroundContactPoints(); ArrayList<GroundContactPoint> groundContactPoints = robot.getAllGroundContactPoints(); for (int i = 0; i < otherGroundContactPoints.size(); i++) { GroundContactPoint otherGroundContactPoint = otherGroundContactPoints.get(i); GroundContactPoint groundContactPoint = groundContactPoints.get(i); Vector3D force = new Vector3D(); otherGroundContactPoint.getForce(force); groundContactPoint.setForce(force); Vector3D moment = new Vector3D(); otherGroundContactPoint.getMoment(moment); groundContactPoint.setMoment(moment); boolean inContact = otherGroundContactPoint.isInContact(); groundContactPoint.setIsInContact(inContact); } }
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); }