/** {@inheritDoc} */ @Override public MovingReferenceFrame getElevatorFrame() { return elevator.getBodyFixedFrame(); }
@Override public ReferenceFrame getHeadBaseFrame() { if(head != null) { JointBasics headJoint = head.getParentJoint(); return headJoint.getFrameAfterJoint(); } return null; }
@Override public String toString() { return "ForceSensorDefinition: " + sensorName + " attached to " + rigidBody.getName(); }
private RigidBodyBasics getRigidBodyHasSameName(FullHumanoidRobotModel fullRobotModel, RigidBodyBasics givenRigidBody) { RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(fullRobotModel.getElevator()); RigidBodyBasics[] allRigidBodies = rootBody.subtreeArray(); for (RigidBodyBasics rigidBody : allRigidBodies) if (givenRigidBody.getName().equals(rigidBody.getName())) return rigidBody; return null; }
/** * Request all the children joints of this rigid-body to update their reference frame and request * their own successor to call this same method. * <p> * By calling this method on the root body of a robot, it will update the reference frames of all * the robot's joints. * </p> */ default void updateFramesRecursively() { getBodyFixedFrame().update(); for (int childIndex = 0; childIndex < getChildrenJoints().size(); childIndex++) { getChildrenJoints().get(childIndex).updateFramesRecursively(); } }
public KinematicsPlanningToolboxCommandConverter(RigidBodyBasics rootBody) { List<ReferenceFrame> referenceFrames = new ArrayList<>(); for (JointBasics joint : rootBody.childrenSubtreeIterable()) { referenceFrames.add(joint.getFrameAfterJoint()); referenceFrames.add(joint.getFrameBeforeJoint()); } for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) { referenceFrames.add(rigidBody.getBodyFixedFrame()); } referenceFrameHashCodeResolver = new ReferenceFrameHashCodeResolver(referenceFrames); for (RigidBodyBasics rigidBody : rootBody.subtreeIterable()) rigidBodyHashMap.put(rigidBody.hashCode(), rigidBody); }
private void printLinkInformation(RigidBodyBasics link, StringBuffer buffer) { SpatialInertiaBasics inertia = link.getInertia(); JointBasics parentJoint = link.getParentJoint(); if (inertia != null) { double mass = inertia.getMass(); Vector3D comOffset = new Vector3D(); RigidBodyTransform comOffsetTransform = link.getBodyFixedFrame().getTransformToDesiredFrame(parentJoint.getFrameAfterJoint()); comOffsetTransform.getTranslation(comOffset); Matrix3DBasics momentOfInertia = inertia.getMomentOfInertia(); buffer.append("Mass = " + mass + "\n"); buffer.append("comOffset = " + comOffset + "\n"); buffer.append("momentOfInertia = \n" + momentOfInertia + "\n"); } List<? extends JointBasics> childrenJoints = link.getChildrenJoints(); for (JointBasics childJoint : childrenJoints) { String parentJointName; if (parentJoint != null) parentJointName = parentJoint.getName(); else parentJointName = "root joint"; buffer.append("Found Child Joint of " + parentJointName + ".\n"); printJointInformation(childJoint, buffer); } }
PlanarRobotArm() { elevator = new RigidBody("elevator", worldFrame); elevatorFrame = elevator.getBodyFixedFrame(); centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMassFrame", ReferenceFrame.getWorldFrame(), elevator); upperArm = createUpperArm(elevator); lowerArm = createLowerArm(upperArm); hand = createHand(lowerArm); scsRobotArm = new SCSRobotFromInverseDynamicsRobotModel("robotArm", elevator.getChildrenJoints().get(0)); scsRobotArm.setGravity(0); scsRobotArm.updateJointPositions_ID_to_SCS(); scsRobotArm.update(); addLinkGraphics(); addForcePoint(); oneDoFJoints = MultiBodySystemTools.createOneDoFJointPath(elevator, hand); elevator.updateFramesRecursively(); }
/** {@inheritDoc} */ @Override public synchronized void updateFrames() { elevator.updateFramesRecursively(); }
@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())); }
@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); }
private void computeNetWrenches() { for (int bodyIndex = 0; bodyIndex < allBodiesExceptRoot.size(); bodyIndex++) { RigidBodyBasics body = allBodiesExceptRoot.get(bodyIndex); Wrench netWrench = netWrenches.get(body); body.getBodyFixedFrame().getTwistOfFrame(tempTwist); if (!doVelocityTerms) tempTwist.setToZero(); tempAcceleration.setIncludingFrame(spatialAccelerationCalculator.getAccelerationOfBody(body)); body.getInertia().computeDynamicWrenchFast(tempAcceleration, tempTwist, netWrench); } }
ReferenceFrame elevatorFrame = elevator.getBodyFixedFrame(); Vector3D jointAxis = new Vector3D(1.0, 0.0, 0.0); PrismaticJoint j1 = new PrismaticJoint("j1", elevator, new Vector3D(), jointAxis); double m1 = r1.getInertia().getMass(); double m2 = r2.getInertia().getMass(); double qdd2 = -(m1 + m2) * qdd1 / m2; j1.setQdd(qdd1); j2.setQdd(qdd2); elevator.updateFramesRecursively();
private Set<JointBasics> getExcludedJoints() { Set<RigidBodyBasics> excludedBodies = getExcludedRigidBodies(); Set<JointBasics> excludedJoints = new LinkedHashSet<JointBasics>(); for (RigidBodyBasics rigidBody : excludedBodies) { excludedJoints.addAll(rigidBody.getChildrenJoints()); } return excludedJoints; }
public static double computeSubTreeMass(RigidBodyBasics rootBody) { SpatialInertiaBasics inertia = rootBody.getInertia(); double ret = inertia == null ? 0.0 : inertia.getMass(); for (JointBasics childJoint : rootBody.getChildrenJoints()) { ret += computeSubTreeMass(childJoint.getSuccessor()); } return ret; }
public CenterOfMassAccelerationCalculator(RigidBodyBasics base, RigidBodyBasics[] rigidBodies, SpatialAccelerationCalculator spatialAccelerationCalculator) { this.spatialAccelerationCalculator = spatialAccelerationCalculator; this.rigidBodies = Stream.of(rigidBodies).filter(body -> body.getInertia() != null).toArray(RigidBodyBasics[]::new); this.base = base; }
public static PassiveRevoluteJoint getFourBarOutputJoint(PassiveRevoluteJoint passiveJointB, PassiveRevoluteJoint passiveJointC, PassiveRevoluteJoint passiveJointD) { // If the output joint is D then it will have at least 1 child, otherwise it won't have any if(passiveJointD.getSuccessor().hasChildrenJoints()) { return passiveJointD; } // Joint C wil only have joint D as its child, unless it's the output joint of the fourbar else if (passiveJointC.getSuccessor().getChildrenJoints().size() > 1) { return passiveJointC; } else { return passiveJointB; } }
/** * Combines {@link #collectSupportJoints(RigidBodyBasics)} with * {@link #collectSubtreeJoints(RigidBodyBasics...)}. * * @param rigidBody the rigid-body to collect the support and subtree joints of. * @return the array containing the support and subtree joints. */ public static JointBasics[] collectSupportAndSubtreeJoints(RigidBodyBasics rigidBody) { List<JointBasics> supportAndSubtreeJoints = new ArrayList<>(); Stream.of(collectSupportJoints(rigidBody)).forEach(supportAndSubtreeJoints::add); rigidBody.childrenSubtreeIterable().forEach(supportAndSubtreeJoints::add); return supportAndSubtreeJoints.toArray(new JointBasics[supportAndSubtreeJoints.size()]); }
public void getCoMAcceleration(FrameVector3D comAccelerationToPack) { boolean firstIteration = true; double totalMass = 0.0; for (RigidBodyBasics rigidBody : rigidBodies) { double mass = rigidBody.getInertia().getMass(); rigidBody.getCenterOfMass(comLocation); linkLinearMomentumDot.setIncludingFrame(spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(base, rigidBody, comLocation)); linkLinearMomentumDot.scale(mass); if (firstIteration) comAccelerationToPack.setIncludingFrame(linkLinearMomentumDot); else comAccelerationToPack.add(linkLinearMomentumDot); totalMass += mass; firstIteration = false; } comAccelerationToPack.scale(1.0 / totalMass); } }
RobotArm() { elevator = new RigidBody("elevator", worldFrame); elevatorFrame = elevator.getBodyFixedFrame(); centerOfMassFrame = new CenterOfMassReferenceFrame("centerOfMass", ReferenceFrame.getWorldFrame(), elevator); shoulderDifferentialYaw = createDifferential("shoulderDifferential", elevator, new Vector3D(), Z); RigidBodyBasics shoulderDifferentialRoll = createDifferential("shoulderDifferential", shoulderDifferentialYaw, new Vector3D(), X); RigidBodyBasics upperArm = createUpperArm(shoulderDifferentialRoll); RigidBodyBasics lowerArm = createLowerArm(upperArm); RigidBodyBasics wristDifferentialRoll = createDifferential("wristDifferential", lowerArm, new Vector3D(0.0, 0.0, SHIN_LENGTH), X); //RigidBody wristDifferentialYaw = createDifferential("wristDifferential", wristDifferentialRoll, new Vector3d(), Z); hand = createHand(wristDifferentialRoll); scsRobotArm = new SCSRobotFromInverseDynamicsRobotModel("robotArm", elevator.getChildrenJoints().get(0)); scsRobotArm.setGravity(0); scsRobotArm.updateJointPositions_ID_to_SCS(); scsRobotArm.update(); addLinkGraphics(); addForcePoint(); oneDoFJoints = MultiBodySystemTools.createOneDoFJointPath(elevator, hand); elevator.updateFramesRecursively(); }