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); } }
public void setRobotExternalForcesRandomly(Random random, double maxGroundContactPointForce, double maxFootExternalForce, double maxFootExternalTorque) { for (RobotSide robotSide : RobotSide.values) { List<GroundContactPoint> footGroundContactPoints = robot.getFootGroundContactPoints(robotSide); for (GroundContactPoint groundContactPoint : footGroundContactPoints) { groundContactPoint.setForce(RandomGeometry.nextVector3D(random, maxGroundContactPointForce)); } ExternalForcePoint footExternalForcePoint = feetExternalForcePoints.get(robotSide); footExternalForcePoint.setForce(RandomGeometry.nextVector3D(random, maxFootExternalForce)); footExternalForcePoint.setMoment(RandomGeometry.nextVector3D(random, maxFootExternalTorque)); } }
double zForce = ground_kp.getDoubleValue() * (pointOnPlane.getZ() - testPoint.getZ()) + ground_kd.getDoubleValue() * gcVelocity.getZ(); gcForce.setZ(gcForce.getZ() + zForce); groundContactPoint.setForce(gcForce);