/** * Creates a new {@code Stream} that consists of all the joint subtrees for each of the {@code root} * children. * * @param root the root of the joint subtree. * @return the new joint subtree stream. */ public static Stream<JointBasics> fromChildren(RigidBodyBasics root) { return from(JointBasics.class, root.getChildrenJoints()); }
private Set<JointBasics> getExcludedJoints() { Set<RigidBodyBasics> excludedBodies = getExcludedRigidBodies(); Set<JointBasics> excludedJoints = new LinkedHashSet<JointBasics>(); for (RigidBodyBasics rigidBody : excludedBodies) { excludedJoints.addAll(rigidBody.getChildrenJoints()); } return excludedJoints; }
default Iterable<? extends JointBasics> childrenSubtreeIterable() { return new JointIterable<>(JointBasics.class, null, this.getChildrenJoints()); }
/** * 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 InverseDynamicsMechanismReferenceFrameVisualizer(RigidBodyBasics rootBody, YoGraphicsListRegistry yoGraphicsListRegistry, double length) { YoGraphicsList yoGraphicsList = new YoGraphicsList(name); List<JointBasics> jointStack = new ArrayList<JointBasics>(rootBody.getChildrenJoints()); while (!jointStack.isEmpty()) { JointBasics joint = jointStack.get(0); ReferenceFrame referenceFrame = joint.getSuccessor().getBodyFixedFrame(); YoGraphicReferenceFrame yoGraphicReferenceFrame = new YoGraphicReferenceFrame(referenceFrame, registry, false, length); yoGraphicsList.add(yoGraphicReferenceFrame); yoGraphicReferenceFrames.add(yoGraphicReferenceFrame); List<? extends JointBasics> childrenJoints = joint.getSuccessor().getChildrenJoints(); jointStack.addAll(childrenJoints); jointStack.remove(joint); } yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); }
/** * Recursively navigates the subtree that starts at the given {@code rootBody} and integrates * each joint acceleration and velocity to update their respective velocity and configuration. * * @param rootBody the origin of the subtree to integrate the state of. The configuration of each * joint in the subtree is modified. */ public void doubleIntegrateFromAccelerationSubtree(RigidBodyBasics rootBody) { if (rootBody == null) return; List<? extends JointBasics> childrenJoints = rootBody.getChildrenJoints(); for (int i = 0; i < childrenJoints.size(); i++) { JointBasics childJoint = childrenJoints.get(i); doubleIntegrateFromAcceleration(childJoint); doubleIntegrateFromAccelerationSubtree(childJoint.getSuccessor()); } }
/** * Generates a random kinematic tree composed of rigid-bodies and prismatic joints. * <p> * The joints and rigid-bodies have random physical parameters. * </p> * <p> * The generated multi-body system is a kinematic tree, i.e. every rigid-body can have one or more * child joint(s). * </p> * * @param random the random generator to use. * @param prefix provides a common prefix used for all the joint and rigid-body names. * @param rootBody the root to which the kinematic tree is to be attached. * @param numberOfJoints how many joints the kinematic tree should be composed of. * @return the list of all the joints composing the kinematic tree. */ public static List<PrismaticJoint> nextPrismaticJointTree(Random random, String prefix, RigidBodyBasics rootBody, int numberOfJoints) { List<PrismaticJoint> prismaticJoints = new ArrayList<>(); RigidBodyBasics predecessor = rootBody; for (int i = 0; i < numberOfJoints; i++) { PrismaticJoint joint = nextPrismaticJoint(random, prefix + "Joint" + i, predecessor); nextRigidBody(random, prefix + "Body" + i, joint); prismaticJoints.add(joint); predecessor = prismaticJoints.get(random.nextInt(prismaticJoints.size())).getSuccessor(); } return SubtreeStreams.from(PrismaticJoint.class, rootBody.getChildrenJoints()).collect(Collectors.toList()); }
/** * Generates a random kinematic tree composed of rigid-bodies and revolute joints. * <p> * The joints and rigid-bodies have random physical parameters. * </p> * <p> * The generated multi-body system is a kinematic tree, i.e. every rigid-body can have one or more * child joint(s). * </p> * * @param random the random generator to use. * @param prefix provides a common prefix used for all the joint and rigid-body names. * @param rootBody the root to which the kinematic tree is to be attached. * @param numberOfJoints how many joints the kinematic tree should be composed of. * @return the list of all the joints composing the kinematic tree. */ public static List<RevoluteJoint> nextRevoluteJointTree(Random random, String prefix, RigidBodyBasics rootBody, int numberOfJoints) { List<RevoluteJoint> revoluteJoints = new ArrayList<>(); RigidBodyBasics predecessor = rootBody; for (int i = 0; i < numberOfJoints; i++) { RevoluteJoint joint = nextRevoluteJoint(random, prefix + "Joint" + i, predecessor); nextRigidBody(random, prefix + "Body" + i, joint); revoluteJoints.add(joint); predecessor = revoluteJoints.get(random.nextInt(revoluteJoints.size())).getSuccessor(); } return SubtreeStreams.from(RevoluteJoint.class, rootBody.getChildrenJoints()).collect(Collectors.toList()); }
/** * Generates a random kinematic tree composed of rigid-bodies and 1-DoF joints. * <p> * The joints and rigid-bodies have random physical parameters. * </p> * <p> * The generated multi-body system is a kinematic tree, i.e. every rigid-body can have one or more * child joint(s). * </p> * * @param random the random generator to use. * @param prefix provides a common prefix used for all the joint and rigid-body names. * @param rootBody the root to which the kinematic tree is to be attached. * @param numberOfJoints how many joints the kinematic tree should be composed of. * @return the list of all the joints composing the kinematic tree. */ public static List<OneDoFJoint> nextOneDoFJointTree(Random random, String prefix, RigidBodyBasics rootBody, int numberOfJoints) { List<OneDoFJoint> oneDoFJoints = new ArrayList<>(); RigidBodyBasics predecessor = rootBody; for (int i = 0; i < numberOfJoints; i++) { OneDoFJoint joint = nextOneDoFJoint(random, prefix + "Joint" + i, predecessor); nextRigidBody(random, prefix + "Body" + i, joint); oneDoFJoints.add(joint); predecessor = oneDoFJoints.get(random.nextInt(oneDoFJoints.size())).getSuccessor(); } return SubtreeStreams.from(OneDoFJoint.class, rootBody.getChildrenJoints()).collect(Collectors.toList()); }
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; } }
/** * Recursively navigates the subtree that starts at the given {@code rootBody} and integrates * each joint velocity to update their respective configuration. * <p> * If the acceleration of each joint is available, it is preferable to use * {@link #doubleIntegrateFromAccelerationSubtree(RigidBodyBasics)}. * </p> * * @param rootBody the origin of the subtree to integrate the state of. The configuration of each * joint in the subtree is modified. */ public void integrateFromVelocitySubtree(RigidBodyBasics rootBody) { if (rootBody == null) return; List<? extends JointBasics> childrenJoints = rootBody.getChildrenJoints(); for (int i = 0; i < childrenJoints.size(); i++) { JointBasics childJoint = childrenJoints.get(i); integrateFromVelocity(childJoint); integrateFromVelocitySubtree(childJoint.getSuccessor()); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void chainTest() throws UnreasonableAccelerationException { Random random = new Random(12651L); ArrayList<RevoluteJoint> joints = new ArrayList<>(); RigidBodyBasics elevator = new RigidBody("elevator", worldFrame); int numberOfJoints = 10; Vector3D[] jointAxes = new Vector3D[numberOfJoints]; for (int i = 0; i < numberOfJoints; i++) jointAxes[i] = RandomGeometry.nextVector3D(random, 1.0); joints.addAll(MultiBodySystemRandomTools.nextRevoluteJointChain(random, "blop", elevator, jointAxes)); SCSRobotFromInverseDynamicsRobotModel robot = new SCSRobotFromInverseDynamicsRobotModel("robot", elevator.getChildrenJoints().get(0)); assertAAndADotV(random, joints, elevator, robot,numberOfJoints); }
public GraphicsIDRobot(String name, RigidBodyBasics rootBody, GraphicsObjectsHolder graphicsObjectsHolder, boolean useCollisionMeshes) { super(new Graphics3DNode(name, Graphics3DNodeType.TRANSFORM)); for (JointBasics joint : rootBody.getChildrenJoints()) { GraphicsJoint rootGraphicsJoint = createJoint(joint, Graphics3DNodeType.ROOTJOINT, graphicsObjectsHolder, useCollisionMeshes); getRootNode().addChild(rootGraphicsJoint); addInverseDynamicsJoints(joint.getSuccessor().getChildrenJoints(), rootGraphicsJoint, graphicsObjectsHolder, useCollisionMeshes); } update(); }
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; }
private void addInverseDynamicsJoints(List<? extends JointBasics> joints, GraphicsJoint parentJoint, GraphicsObjectsHolder graphicsObjectsHolder, boolean useCollisionMeshes) { for (JointBasics joint : joints) { GraphicsJoint graphicsJoint = createJoint(joint, Graphics3DNodeType.JOINT, graphicsObjectsHolder, useCollisionMeshes); parentJoint.addChild(graphicsJoint); addInverseDynamicsJoints(joint.getSuccessor().getChildrenJoints(), graphicsJoint, graphicsObjectsHolder, useCollisionMeshes); } }
public JointAxisVisualizer(RigidBodyBasics rootBody, YoGraphicsListRegistry yoGraphicsListRegistry, double length) { YoGraphicsList yoGraphicsList = new YoGraphicsList(name); List<JointBasics> jointStack = new ArrayList<JointBasics>(rootBody.getChildrenJoints()); while (!jointStack.isEmpty()) { JointBasics joint = jointStack.get(0); if(joint instanceof OneDoFJointBasics) { FrameVector3DReadOnly jAxis=((OneDoFJointBasics)joint).getJointAxis(); ReferenceFrame referenceFrame = GeometryTools.constructReferenceFrameFromPointAndZAxis(joint.getName()+"JointAxis", new FramePoint3D(jAxis.getReferenceFrame()), new FrameVector3D(jAxis.getReferenceFrame(),jAxis)); YoGraphicReferenceFrame yoGraphicReferenceFrame = new YoGraphicReferenceFrame(referenceFrame , registry, false, length, YoAppearance.Gold()); yoGraphicsList.add(yoGraphicReferenceFrame); yoGraphicReferenceFrames.add(yoGraphicReferenceFrame); } List<? extends JointBasics> childrenJoints = joint.getSuccessor().getChildrenJoints(); jointStack.addAll(childrenJoints); jointStack.remove(joint); } yoGraphicsListRegistry.registerYoGraphicsList(yoGraphicsList); }
jointWrench.sub(externalWrench); List<? extends JointBasics> childrenJoints = successor.getChildrenJoints();
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(); }
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(); }