/** * Builds a new root body. * * @param bodyName the body's name. * @param transformToParent provides the pose of this rigid-body's body-fixed-frame with respect to * the parentStationaryFrame. * @param parentStationaryFrame the parent stationary, i.e. non-moving with respect to world frame, * frame to which this rigid-body will create and attach its body fixed frame. * @return the new root body. */ default RigidBodyBasics buildRoot(String bodyName, RigidBodyTransform transformToParent, ReferenceFrame parentStationaryFrame) { return new RigidBody(bodyName, transformToParent, parentStationaryFrame); }
/** * Randomizes the state of this multi-body system and updates its reference frames. * * @param random the random generator to use. * @param stateSelections the states to randomize. */ public void nextState(Random random, JointStateType... stateSelections) { for (JointStateType selection : stateSelections) MultiBodySystemRandomTools.nextState(random, selection, getJoints()); getElevator().updateFramesRecursively(); }
/** * Builds a new rigid-body. * * @param bodyName the body's name. * @param parentJoint the joint directly attached to this rigid-body and located between this * rigid-body and the root body of the robot. * @param momentOfInertia the 3D moment of inertia of this rigid-body. * @param mass the mass of this rigid-body. * @param inertiaPose defines the transform from this rigid-body body-fixed-frame to the * parentJoint.getFrameAfterJointFrame(). The given moment of inertia is assumed to be * expressed in that body-fixed-frame. Also note that the translation part corresponds to * the position of this rigid-body center of mass position expressed in * parentJoint.getFrameAfterJointFrame(). * @return the new rigid-body. */ default RigidBodyBasics build(String bodyName, JointBasics parentJoint, Matrix3DReadOnly momentOfInertia, double mass, RigidBodyTransform inertiaPose) { return new RigidBody(bodyName, parentJoint, momentOfInertia, mass, inertiaPose); } }
MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, revoluteJoints); MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, -10.0, 10.0, revoluteJoints); floatingChain.getElevator().updateFramesRecursively();
@Before public void setUp() { elevator = new RigidBody("elevator", worldFrame); }
MultiBodySystemStateIntegrator integrator = new MultiBodySystemStateIntegrator(dt); integrator.integrateFromVelocity(randomFloatingChain.getRevoluteJoints()); randomFloatingChain.getElevator().updateFramesRecursively(); point2.changeFrame(base.getBodyFixedFrame());
public TestingRobotModel() { worldFrame = ReferenceFrame.getWorldFrame(); elevator = new RigidBody("elevator", worldFrame); elevatorFrame = elevator.getBodyFixedFrame(); rootJoint = new SixDoFJoint("rootJoint", elevator); body = new RigidBody("body", rootJoint, Ixx, Iyy, Izz, mass, comOffset); bodyFrame = rootJoint.getFrameAfterJoint(); }
/** * Generates a rigid-body with random physical parameters and attaches it to the given * {@code parentJoint}. * * @param random the random generator to use. * @param name the rigid-body name. * @param parentJoint the joint to which the rigid-body is added as its successor. * @return the random rigid-body. */ public static RigidBody nextRigidBody(Random random, String name, JointBasics parentJoint) { Matrix3D momentOfInertia = MecanoRandomTools.nextSymmetricPositiveDefiniteMatrix3D(random, 1.0e-4, 2.0, 0.5); double mass = 0.1 + random.nextDouble(); Vector3D comOffset = EuclidCoreRandomTools.nextVector3D(random); return new RigidBody(name, parentJoint, momentOfInertia, mass, comOffset); }
private RigidBodyBasics createDifferential(String name, RigidBodyBasics parentBody, Vector3D jointOffset, Vector3D jointAxis) { String jointName; if (jointAxis == X) jointName = name + "_x"; else if (jointAxis == Y) jointName = name + "_y"; else jointName = name + "_z"; RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, jointOffset, jointAxis); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(); return new RigidBody(jointName, joint, 0.005, 0.005, 0.005, 0.1, comOffset); }
private RigidBodyBasics createDifferential(String name, RigidBodyBasics parentBody, Vector3D jointOffset, Vector3D jointAxis) { String jointName; if (jointAxis == X) jointName = name + "_x"; else if (jointAxis == Y) jointName = name + "_y"; else jointName = name + "_z"; RevoluteJoint joint = new RevoluteJoint(jointName, parentBody, jointOffset, jointAxis); joint.setQ(random.nextDouble()); Vector3D comOffset = new Vector3D(); return new RigidBody(name, joint, 0.005, 0.005, 0.005, 0.1, comOffset); }
/** * 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 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, int numberOfJoints) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); RigidBody rootBody = new RigidBody(prefix + "RootBody", worldFrame); return nextPrismaticJointTree(random, prefix, rootBody, numberOfJoints); }
/** * Generates a random kinematic chain 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 chain, i.e. every rigid-body has only one child * joint. * </p> * * @param random the random generator to use. * @param prefix provides a common prefix used for all the joint and rigid-body names. * @param jointAxes array containing in order the axis for each joint. The length of the array also * defines the number of joints for the generated kinematic chain. * @return the list of all the joints composing the kinematic chain. */ public static List<PrismaticJoint> nextPrismaticJointChain(Random random, String prefix, Vector3DReadOnly[] jointAxes) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); RigidBody rootBody = new RigidBody(prefix + "RootBody", worldFrame); return nextPrismaticJointChain(random, prefix, rootBody, jointAxes); }
/** * Generates a random kinematic chain 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 chain, i.e. every rigid-body has only one child * joint. * </p> * * @param random the random generator to use. * @param prefix provides a common prefix used for all the joint and rigid-body names. * @param jointAxes array containing in order the axis for each joint. The length of the array also * defines the number of joints for the generated kinematic chain. * @return the list of all the joints composing the kinematic chain. */ public static List<RevoluteJoint> nextRevoluteJointChain(Random random, String prefix, Vector3DReadOnly[] jointAxes) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); RigidBody rootBody = new RigidBody(prefix + "RootBody", worldFrame); return nextRevoluteJointChain(random, prefix, rootBody, jointAxes); }
/** * 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 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, int numberOfJoints) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); RigidBody rootBody = new RigidBody("RootBody", worldFrame); return nextRevoluteJointTree(random, prefix, rootBody, numberOfJoints); }
/** * 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 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, int numberOfJoints) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); RigidBody rootBody = new RigidBody("RootBody", worldFrame); return nextOneDoFJointTree(random, prefix, rootBody, numberOfJoints); }
/** * Generates a random kinematic chain 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 chain, i.e. every rigid-body has only one child * joint. * </p> * * @param random the random generator to use. * @param prefix provides a common prefix used for all the joint and rigid-body names. * @param jointAxes array containing in order the axis for each joint. The length of the array also * defines the number of joints for the generated kinematic chain. * @return the list of all the joints composing the kinematic chain. */ public static List<OneDoFJoint> nextOneDoFJointChain(Random random, String prefix, Vector3DReadOnly[] jointAxes) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); RigidBody rootBody = new RigidBody(prefix + "RootBody", worldFrame); return nextOneDoFJointChain(random, prefix, rootBody, jointAxes); }
private static RigidBodyBasics createAndAttachCylinderRB(String name, RevoluteJoint parentJoint) { Matrix3D inertiaCylinder = RotationalInertiaCalculator.getRotationalInertiaMatrixOfSolidCylinder(1.0, 1.0, 1.0, Axis.Z); return new RigidBody(name, parentJoint, inertiaCylinder, 1.0, new Vector3D()); }
public MassMatrixCalculatorComparer() elevator = new RigidBody("elevator", worldFrame); Vector3D[] jointAxes = {X, Y, Z, Z, X, Z, Z, X, Y, Y}; joints.addAll(MultiBodySystemRandomTools.nextRevoluteJointChain(random, "", elevator, jointAxes));
/** * Generates a random kinematic tree composed of rigid-bodies and 1-DoF joints. * <p> * The type of each joint is chosen at random. * </p> * <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 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<JointBasics> nextJointTree(Random random, String prefix, int numberOfJoints) { ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); RigidBody rootBody = new RigidBody("RootBody", worldFrame); return nextJointTree(random, prefix, rootBody, numberOfJoints); }
/** * Creates a new random multi-body system. * * @param random the random generator to use. * @param jointAxes array containing in order the axis for each revolute joint. The length of the * array also defines the number of revolute joints for the generated kinematic chain. */ public RandomFloatingRevoluteJointChain(Random random, Vector3D[] jointAxes) { elevator = new RigidBody("elevator", ReferenceFrame.getWorldFrame()); rootJoint = new SixDoFJoint("rootJoint", elevator); RigidBody rootBody = nextRigidBody(random, "rootBody", rootJoint); revoluteJoints = nextRevoluteJointChain(random, "test", rootBody, jointAxes); joints.add(rootJoint); joints.addAll(revoluteJoints); }