/** * Retrieves and gets the root body of the multi-body system the given {@code body} belongs to. * * @param body an arbitrary body that belongs to the multi-body system that this method is to find * the root. * @return the root body. */ public static RigidBodyBasics getRootBody(RigidBodyBasics body) { RigidBodyBasics root = body; while (root.getParentJoint() != null) { root = root.getParentJoint().getPredecessor(); } return root; }
@Override public ReferenceFrame getHeadBaseFrame() { if(head != null) { JointBasics headJoint = head.getParentJoint(); return headJoint.getFrameAfterJoint(); } return null; }
public String getParentJointName() { return contactingRigidBody.getParentJoint().getName(); }
@Override public ReferenceFrame getHeadBaseFrame() { return head.getParentJoint().getFrameAfterJoint(); }
@Override public void getOneDoFJointsFromRootToHere(OneDoFJointBasics oneDoFJointAtEndOfChain, List<OneDoFJointBasics> oneDoFJointsToPack) { oneDoFJointsToPack.clear(); JointBasics parent = oneDoFJointAtEndOfChain; while (parent != rootJoint) { if (parent instanceof OneDoFJointBasics) { oneDoFJointsToPack.add((OneDoFJointBasics) parent); } parent = parent.getPredecessor().getParentJoint(); } Collections.reverse(oneDoFJointsToPack); }
@Override public void getOneDoFJointsFromRootToHere(OneDoFJointBasics oneDoFJointAtEndOfChain, List<OneDoFJointBasics> oneDoFJointsToPack) { oneDoFJointsToPack.clear(); JointBasics parent = oneDoFJointAtEndOfChain; while (parent != rootJoint) { if (parent instanceof OneDoFJointBasics) { oneDoFJointsToPack.add((OneDoFJointBasics) parent); } parent = parent.getPredecessor().getParentJoint(); } Collections.reverse(oneDoFJointsToPack); }
/** {@inheritDoc} */ @Override public MovingReferenceFrame getEndEffectorFrame(RobotSide robotSide, LimbName limbName) { return getEndEffector(robotSide, limbName).getParentJoint().getFrameAfterJoint(); }
@Override public MovingReferenceFrame getEndEffectorFrame(RobotQuadrant robotQuadrant, LimbName limbName) { if (hasQuadrant(robotQuadrant)) return getEndEffector(robotQuadrant, limbName).getParentJoint().getFrameAfterJoint(); else return null; }
public IMUDefinition(String name, RigidBodyBasics rigidBody, RigidBodyTransform transformFromIMUToJoint) { this.name = name; this.rigidBody = rigidBody; this.transformFromIMUToJoint = new RigidBodyTransform(transformFromIMUToJoint); ReferenceFrame frameAfterJoint = rigidBody.getParentJoint().getFrameAfterJoint(); imuFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent(name, frameAfterJoint, transformFromIMUToJoint); }
public PushRobotController(FloatingRootJointRobot pushableRobot, FullHumanoidRobotModel fullRobotModel) { this(pushableRobot, fullRobotModel.getChest().getParentJoint().getName(), new Vector3D(0, 0, 0.3), 0.005); }
private SideDependentList<String> getFootJointNames(FullHumanoidRobotModel fullRobotModel) { SideDependentList<String> jointNames = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { jointNames.put(robotSide, fullRobotModel.getFoot(robotSide).getParentJoint().getName()); } return jointNames; }
public ForceSensorDefinition(String sensorName, RigidBodyBasics rigidBody, RigidBodyTransform transformFromSensorToParentJoint) { this.sensorName = sensorName; this.rigidBody = rigidBody; JointBasics parentJoint = rigidBody.getParentJoint(); this.parentJointName = parentJoint.getName(); this.transformFromSensorToParentJoint = new RigidBodyTransform(transformFromSensorToParentJoint); ReferenceFrame frameAfterJoint = parentJoint.getFrameAfterJoint(); sensorFrame = ReferenceFrame.constructFrameWithUnchangingTransformToParent(sensorName + "Frame", frameAfterJoint, transformFromSensorToParentJoint); }
private SideDependentList<String> getFootJointNames(FullHumanoidRobotModel fullRobotModel) { SideDependentList<String> jointNames = new SideDependentList<>(); for (RobotSide robotSide : RobotSide.values) { jointNames.put(robotSide, fullRobotModel.getFoot(robotSide).getParentJoint().getName()); } return jointNames; }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddRigidBody_String_InverseDynamicsJoint_Matrix3d_double_Vector3d() { String name = "body"; RigidBodyBasics predecessor = new RigidBody("Predecessor", theFrame); PlanarJoint parentJoint = new PlanarJoint(name, predecessor); Matrix3D momentOfInertia = new Matrix3D(); double mass = random.nextDouble(); RigidBodyBasics body = new RigidBody(name, parentJoint, momentOfInertia, mass, X); assertEquals("Should be equal", name, body.getName()); assertTrue(parentJoint.equals(body.getParentJoint())); }
public void holdCurrentPelvisHeight() { yoDesiredPelvisPosition.setFromReferenceFrame(fullRobotModel.getPelvis().getParentJoint().getFrameAfterJoint()); pelvisSelectionMatrix.clearLinearSelection(); pelvisSelectionMatrix.selectLinearZ(true); pelvisSelectionMatrix.setSelectionFrame(worldFrame); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testAddRigidBody_String_InverseDynamicsJoint_Matrix3d_double_Transform3D() { String name = "body"; RigidBodyBasics predecessor = new RigidBody("Predecessor", theFrame); PlanarJoint parentJoint = new PlanarJoint(name, predecessor); Matrix3D momentOfInertia = new Matrix3D(); double mass = random.nextDouble(); RigidBodyTransform inertiaPose = new RigidBodyTransform(); RigidBodyBasics body = new RigidBody(name, parentJoint, momentOfInertia, mass, inertiaPose); assertEquals("Should be equal", name, body.getName()); assertTrue(parentJoint.equals(body.getParentJoint())); }
public ScriptedFootstepGenerator(HumanoidReferenceFrames referenceFrames, FullHumanoidRobotModel fullRobotModel, WalkingControllerParameters walkingControllerParameters) { this.bipedFeet = setupBipedFeet(referenceFrames, fullRobotModel, walkingControllerParameters); for (RobotSide robotSide : RobotSide.values) { RigidBodyBasics foot = bipedFeet.get(robotSide).getRigidBody(); ReferenceFrame ankleFrame = foot.getParentJoint().getFrameAfterJoint(); RigidBodyTransform ankleToSole = new RigidBodyTransform(); ReferenceFrame soleFrame = referenceFrames.getSoleFrame(robotSide); ankleFrame.getTransformToDesiredFrame(ankleToSole, soleFrame); transformsFromAnkleToSole.put(robotSide, ankleToSole); } }
@Before public void setUp() throws Exception { transformIMUToJoint.setRotation(jointToIMURotation); transformIMUToJoint.setTranslation(jointToIMUOffset); transformJointToIMU.setAndInvert(transformIMUToJoint); imuFrame = fullRobotModel.createOffsetFrame(fullRobotModel.getBodyLink().getParentJoint(), transformIMUToJoint, "imuFrame"); Vector3D linearAcceleration = new Vector3D(0.0, 0.0, GRAVITY); Vector3D angularAcceleration = new Vector3D(); ReferenceFrame rootBodyFrame = fullRobotModel.getElevatorFrame(); SpatialAcceleration rootAcceleration = new SpatialAcceleration(rootBodyFrame, ReferenceFrame.getWorldFrame(), rootBodyFrame, angularAcceleration, linearAcceleration); simulatedIMURawSensorReader = new PerfectSimulatedIMURawSensorReader(rawSensors, IMU_INDEX, rigidBody, imuFrame, fullRobotModel.getElevator(), rootAcceleration); simulatedIMURawSensorReader.initialize(); }
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testVMC() { RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity); RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT); RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor(); RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor(); // send in the correct frame and no selection matrix FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame()); FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame()); Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce); submitAndCheckVMC(pelvis, foot, desiredWrench, null); }
@ContinuousIntegrationTest(estimatedDuration = 0.6) @Test(timeout = 30000) public void testVMCSelectAll() { RobotLegs robotLeg = VirtualModelMomentumControllerTestHelper.createRobotLeg(gravity); RigidBodyBasics endEffector = robotLeg.getFoot(RobotSide.LEFT); RigidBodyBasics foot = endEffector.getParentJoint().getSuccessor(); RigidBodyBasics pelvis = robotLeg.getRootJoint().getSuccessor(); // send in the correct frame with identity selection matrix FrameVector3D desiredForce = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame()); FrameVector3D desiredTorque = EuclidFrameRandomTools.nextFrameVector3D(random, foot.getBodyFixedFrame()); Wrench desiredWrench = new Wrench(foot.getBodyFixedFrame(), foot.getBodyFixedFrame(), desiredTorque, desiredForce); SelectionMatrix6D selectionMatrix = new SelectionMatrix6D(); submitAndCheckVMC(pelvis, foot, desiredWrench, selectionMatrix); }