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); } }
groundContactPoint.getForce(force); groundContactPoint.getForce(forceWithNormalsDisabled); groundContactModel.doGroundContact(); assertTrue(groundContactPoint.isInContact()); groundContactPoint.getForce(force); groundContactPoint.getForce(forceWithNormalsDisabled); groundContactModel.doGroundContact(); assertFalse(groundContactPoint.isInContact()); groundContactPoint.getForce(force);
groundContactPoint.getForce(gcForce);
groundContactPoint.getForce(force);