public MassMatrixCalculatorComparer() { joints = new ArrayList<RevoluteJoint>(); elevator = new RigidBody("elevator", worldFrame); Vector3d[] jointAxes = {X, Y, Z, Z, X, Z, Z, X, Y, Y}; ScrewTestTools.createRandomChainRobot("", joints, elevator, jointAxes, random); massMatrixCalculators.add(new DifferentialIDMassMatrixCalculator(worldFrame, elevator)); massMatrixCalculators.add(new CompositeRigidBodyMassMatrixCalculator(elevator)); }
public static RigidBody addRigidBody(String name, InverseDynamicsJoint parentJoint, Matrix3d momentOfInertia, double mass, Vector3d centerOfMassOffset) { String comFrameName = name + "CoM"; ReferenceFrame comFrame = createOffsetFrame(parentJoint.getFrameAfterJoint(), centerOfMassOffset, comFrameName); RigidBodyInertia inertia = new RigidBodyInertia(comFrame, momentOfInertia, mass); RigidBody ret = new RigidBody(name, inertia, parentJoint); return ret; }
public static RigidBody addRigidBody(String name, InverseDynamicsJoint parentJoint, Matrix3d momentOfInertia, double mass, RigidBodyTransform inertiaPose) { String comFrameName = name + "CoM"; ReferenceFrame comFrame = createOffsetFrame(parentJoint.getFrameAfterJoint(), inertiaPose, comFrameName); RigidBodyInertia inertia = new RigidBodyInertia(comFrame, momentOfInertia, mass); RigidBody ret = new RigidBody(name, inertia, parentJoint); return ret; }
public RandomFloatingChain(Random random, Vector3d[] jointAxes) { ReferenceFrame elevatorFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent("elevator", ReferenceFrame.getWorldFrame(), new RigidBodyTransform()); elevator = new RigidBody("elevator", elevatorFrame); rootJoint = new SixDoFJoint("rootJoint", elevator, elevatorFrame); RigidBody rootBody = ScrewTestTools.addRandomRigidBody("rootBody", random, rootJoint); revoluteJoints = new ArrayList<RevoluteJoint>(); ScrewTestTools.createRandomChainRobot("test", revoluteJoints, rootBody, jointAxes, random); inverseDynamicsJoints.add(rootJoint); inverseDynamicsJoints.addAll(revoluteJoints); }
public static List<OneDoFJoint> createRandomChainRobotWithOneDoFJoints(String prefix, Vector3d[] jointAxes, Random random) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame rootBodyFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent("rootBodyFrame", worldFrame, new RigidBodyTransform()); RigidBody rootBody = new RigidBody(prefix + "RootBody", rootBodyFrame); return createRandomChainRobotWithOneDoFJoints(prefix, rootBody, jointAxes, random); }
public static List<PrismaticJoint> createRandomChainRobotWithPrismaticJoints(String prefix, Vector3d[] jointAxes, Random random) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame rootBodyFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent("rootBodyFrame", worldFrame, new RigidBodyTransform()); RigidBody rootBody = new RigidBody(prefix + "RootBody", rootBodyFrame); return createRandomChainRobotWithPrismaticJoints(prefix, rootBody, jointAxes, random); }
public static List<PrismaticJoint> createRandomTreeRobotWithPrismaticJoints(String prefix, int numberOfJoints, Random random) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame rootBodyFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent("rootBodyFrame", worldFrame, new RigidBodyTransform()); RigidBody rootBody = new RigidBody(prefix + "RootBody", rootBodyFrame); return createRandomTreeRobotWithPrismaticJoints(prefix, rootBody, numberOfJoints, random); }
public static List<OneDoFJoint> createRandomTreeRobotWithOneDoFJoints(String prefix, int numberOfJoints, Random random) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame rootBodyFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent("rootBodyFrame", worldFrame, new RigidBodyTransform()); RigidBody rootBody = new RigidBody(prefix + "RootBody", rootBodyFrame); return createRandomTreeRobotWithOneDoFJoints(prefix, rootBody, numberOfJoints, random); }
public static List<RevoluteJoint> createRandomChainRobot(String prefix, Vector3d[] jointAxes, Random random) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame rootBodyFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent("rootBodyFrame", worldFrame, new RigidBodyTransform()); RigidBody rootBody = new RigidBody(prefix + "RootBody", rootBodyFrame); return createRandomChainRobot(prefix, rootBody, jointAxes, random); }
public static List<RevoluteJoint> createRandomTreeRobot(int numberOfJoints, Random random) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); ReferenceFrame rootBodyFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent("rootBodyFrame", worldFrame, new RigidBodyTransform()); RigidBody rootBody = new RigidBody("RootBody", rootBodyFrame); return createRandomTreeRobot(rootBody, numberOfJoints, random); }
public static InverseDynamicsJoint[] cloneJointPathDisconnectedFromOriginalRobot(InverseDynamicsJoint[] inverseDynamicsJoints, String suffix, ReferenceFrame rootBodyFrame) { InverseDynamicsJoint[] cloned = new InverseDynamicsJoint[inverseDynamicsJoints.length]; for (int i = 0; i < inverseDynamicsJoints.length; i++) { if (inverseDynamicsJoints[i] instanceof RevoluteJoint) { RevoluteJoint jointOriginal = (RevoluteJoint) inverseDynamicsJoints[i]; RigidBody predecessorOriginal = jointOriginal.getPredecessor(); RigidBody predecessorCopy; if (i > 0) { predecessorCopy = cloned[i - 1].getSuccessor(); } else { String predecessorNameOriginal = predecessorOriginal.getName(); predecessorCopy = new RigidBody(predecessorNameOriginal + suffix, rootBodyFrame); } cloned[i] = cloneOneDoFJoint(jointOriginal, suffix, predecessorCopy); } else { throw new RuntimeException("Not implemented for joints of the type: " + inverseDynamicsJoints[i].getClass().getSimpleName()); } cloneRigidBody(inverseDynamicsJoints[i].getSuccessor(), suffix, cloned[i]); } return cloned; }
predecessorCopy = new RigidBody(predecessorNameOriginal + suffix, predecessorFrameAfterParentJointOriginal); originalToClonedRigidBodies.put(predecessorOriginal, predecessorCopy); RigidBody rootBodyCopy = new RigidBody(rootBodyNameOriginal + suffix, rootBodyFrame); originalToClonedRigidBodies.put(rootBody, rootBodyCopy);
this.shin = new RigidBody(name, shinFrame); this.ankle = ScrewTools.addRevoluteJoint(name + "Ankle", shin, new RigidBodyTransform(), new Vector3d(0.0, 1.0, 0.0)); this.foot = ScrewTools.addRigidBody(name, ankle, new Matrix3d(), 1.0, new RigidBodyTransform());
public FootSpoof(String name, double xToAnkle, double yToAnkle, double zToAnkle, List<Point2d> contactPoints2dInSoleFrame, double coefficientOfFriction) { RigidBodyTransform transformToAnkle = new RigidBodyTransform(); transformToAnkle.setTranslation(new Vector3d(-xToAnkle, -yToAnkle, -zToAnkle)); // if(FootstepUtilsTest.DEBUG_TESTS) // System.out.println("FootSpoof: making transform from plane to ankle equal to "+transformToAnkle); shinFrame = new PoseReferenceFrame(name + "ShinFrame", ReferenceFrame.getWorldFrame()); this.shin = new RigidBody(name, shinFrame); this.ankle = ScrewTools.addRevoluteJoint(name + "Ankle", shin, new RigidBodyTransform(), new Vector3d(0.0, 1.0, 0.0)); this.foot = ScrewTools.addRigidBody(name, ankle, new Matrix3d(), 1.0, new RigidBodyTransform()); soleFrame = ReferenceFrame.constructBodyFrameWithUnchangingTransformToParent(name + "soleFrame", ankle.getFrameAfterJoint(), transformToAnkle); for (Point2d contactPointInSoleFrame : contactPoints2dInSoleFrame) { FramePoint point = new FramePoint(soleFrame, contactPointInSoleFrame.getX(), contactPointInSoleFrame.getY(), 0.0); contactPoints.add(point); contactPoints2d.add(point.toFramePoint2d()); } totalNumberOfContactPoints = contactPoints.size(); this.coefficientOfFriction = coefficientOfFriction; }
SideDependentList<SixDoFJoint> sixDoFJoints = new SideDependentList<SixDoFJoint>(); RigidBody rootBody = new RigidBody("Root", ReferenceFrame.getWorldFrame()); Random random = new Random(1776L);
elevator = new RigidBody("elevator", elevatorFrame);
public static void main(String[] args) RigidBody elevator = new RigidBody("elevator", ReferenceFrame.getWorldFrame()); SixDoFJoint rootJoint = new SixDoFJoint("sixdof", elevator, ReferenceFrame.getWorldFrame()); RigidBodyTransform inertiaPose = new RigidBodyTransform();
public ProvidedMassMatrixToolRigidBody(RobotSide robotSide, final FullHumanoidRobotModel fullRobotModel, double gravity, ArmControllerParameters armControllerParameters, YoVariableRegistry parentRegistry, YoGraphicsListRegistry yoGraphicsListRegistry) { String name = robotSide.getCamelCaseNameForStartOfExpression() + "Tool"; this.registry = new YoVariableRegistry(name); this.fullRobotModel = fullRobotModel; this.gravity = gravity; this.handFixedFrame = fullRobotModel.getHand(robotSide).getBodyFixedFrame(); this.handControlFrame = fullRobotModel.getHandControlFrame(robotSide); this.elevatorFrame = fullRobotModel.getElevatorFrame(); toolFrame = new PoseReferenceFrame(name + "Frame", elevatorFrame); RigidBodyInertia inertia = new RigidBodyInertia(toolFrame, new Matrix3d(), 0.0); this.toolJoint = new SixDoFJoint(name + "Joint", fullRobotModel.getElevator(), fullRobotModel.getElevator().getBodyFixedFrame()); this.toolBody = new RigidBody(name + "Body", inertia, toolJoint); objectCenterOfMass = new YoFramePoint(name + "CoMOffset", handControlFrame, registry); objectMass = new DoubleYoVariable(name + "ObjectMass", registry); objectForceInWorld = new YoFrameVector(name + "Force", ReferenceFrame.getWorldFrame(), registry); this.objectCenterOfMassInWorld = new YoFramePoint(name + "CoMInWorld", ReferenceFrame.getWorldFrame(), registry); if (yoGraphicsListRegistry != null) { YoGraphicsList yoGraphicsList = new YoGraphicsList(name); YoGraphic comViz = new YoGraphicPosition(name + "CenterOfMassViz", objectCenterOfMassInWorld, 0.05, YoAppearance.Red()); yoGraphicsList.add(comViz); YoGraphic vectorViz = new YoGraphicVector(name + "ForceViz", objectCenterOfMassInWorld, objectForceInWorld, YoAppearance.Yellow()); yoGraphicsList.add(vectorViz); yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); } parentRegistry.addChild(registry); }