/** * Performs a deep copy of the given {@code originalJoints}. * <p> * The {@code originalJoints} must represent a continuous kinematic chain. The joints must be stored * in order starting from the joint that is the closest to the root, to end with the joint the * closest to an end-effector. * </p> * <p> * The clone of the kinematic chain has its own root body which reference frame is child of the * frame after the parent joint of {@code originalJoints[0]}. As a result, the clone is an * independent multi-body system but its root is following the original multi-body system. * </p> * * @param originalJoints the kinematic chain to clone. Not modified. * @return the clone kinematic chain. */ public static JointBasics[] cloneKinematicChain(JointReadOnly[] originalJoints) { return cloneKinematicChain(originalJoints, "Copy"); }
/** * Performs a deep copy of an entire multi-body system. * <p> * The clone of the multi-body system has its own root body which reference frame shares the same * parent as {@code originalRootBody.getBodyFixedFrame()}. * </p> * * @param originalRootBody the root of the multi-body system to clone. Not modified. * @param cloneStationaryFrame the reference frame to which the cloned system is attached to. The * given frame is expected to be stationary. * @param cloneSuffix suffix to append to the cloned joints and rigid-bodies. * @return the clone multi-body system. * @throws IllegalArgumentException if the given {@code originalRootBody} is not the root body of * its system. */ public static RigidBodyBasics cloneMultiBodySystem(RigidBodyReadOnly originalRootBody, ReferenceFrame cloneStationaryFrame, String cloneSuffix) { return cloneMultiBodySystem(originalRootBody, cloneStationaryFrame, cloneSuffix, null, null); }
public ControllerSpy(OneDoFJointBasics[] spineJoints, SimulationConstructionSet scs, double controllerDT) { this.spineJoints = spineJoints; this.controllerDT = controllerDT; spineJointClones = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(spineJoints); chestClone = spineJointClones[spineJointClones.length - 1].getSuccessor(); for (int jointIdx = 0; jointIdx < numberOfJoints; jointIdx++) { OneDoFJointBasics joint = spineJoints[jointIdx]; jointDesiredsMap.put(joint, findJointDesired(scs, joint)); jointControlEnabled.put(joint, findJointControlEnabled(scs, joint)); } orientationControlEnabled = findOrientationControlEnabled(scs, chest); desiredOrientation = findOrientationDesired(scs, chest); inconsistentControl.set(false); maxSpeed.set(0.0); }
private static JointBasics cloneJoint(JointReadOnly original, String cloneSuffix, RigidBodyBasics clonePredecessor, boolean testIfPredecessorIsRoot, JointBuilder jointBuilder) { if (jointBuilder == null) jointBuilder = DEFAULT_JOINT_BUILDER; if (original instanceof OneDoFJointReadOnly) return cloneOneDoFJoint((OneDoFJointReadOnly) original, cloneSuffix, clonePredecessor, jointBuilder); else if (original instanceof SixDoFJointReadOnly) return cloneSixDoFJoint((SixDoFJointReadOnly) original, cloneSuffix, clonePredecessor, jointBuilder); else if (original instanceof PlanarJointReadOnly) return clonePlanarJoint((PlanarJointReadOnly) original, cloneSuffix, clonePredecessor, jointBuilder); else if (original instanceof SphericalJointReadOnly) return cloneSphericalJoint((SphericalJointReadOnly) original, cloneSuffix, clonePredecessor, jointBuilder); else if (original instanceof FixedJointReadOnly) return cloneFixedJoint((FixedJointReadOnly) original, cloneSuffix, clonePredecessor, jointBuilder); else throw new UnsupportedOperationException("Unhandled joint type: " + original.getClass().getName()); }
private static void cloneSubtree(RigidBodyReadOnly originalStart, RigidBodyBasics cloneStart, String cloneSuffix, RigidBodyBuilder rigidBodyBuilder, JointBuilder jointBuilder) { Map<RigidBodyReadOnly, RigidBodyBasics> originalToCloneBodyMap = new HashMap<>(); originalToCloneBodyMap.put(originalStart, cloneStart); for (JointReadOnly originalJoint : originalStart.childrenSubtreeIterable()) { RigidBodyReadOnly originalPredecessor = originalJoint.getPredecessor(); // Retrieve the right predecessor for the joint to clone. The map has to contain the clone predecessor. RigidBodyBasics clonePredecessor = originalToCloneBodyMap.get(originalPredecessor); // Clone the joint JointBasics cloneJoint = cloneJoint(originalJoint, cloneSuffix, clonePredecessor, originalStart.isRootBody(), jointBuilder); // Clone the successor RigidBodyReadOnly originalSuccessor = originalJoint.getSuccessor(); RigidBodyBasics cloneSuccessor = cloneRigidBody(originalSuccessor, null, cloneSuffix, cloneJoint, rigidBodyBuilder); originalToCloneBodyMap.put(originalSuccessor, cloneSuccessor); } }
/** * Performs a deep copy of the given {@code originalJoints} then filters the cloned joint to return * only the ones implementing {@code OneDoFJoint}. * <p> * The {@code originalJoints} must represent a continuous kinematic chain. The joints must be stored * in order starting from the joint that is the closest to the root, to end with the joint the * closest to an end-effector. * </p> * <p> * The clone of the kinematic chain has its own root body which reference frame is child of the * frame after the parent joint of {@code originalJoints[0]}. As a result, the clone is an * independent multi-body system but its root is following the original multi-body system. * </p> * * @param originalJoints the kinematic chain to clone. Not modified. * @return the clone kinematic chain. */ public static OneDoFJointBasics[] cloneOneDoFJointKinematicChain(OneDoFJointBasics[] originalJoints) { return cloneKinematicChainAndFilter(originalJoints, OneDoFJointBasics.class); }
RigidBodyBasics cloneAncestor; if (originalAncestor.isRootBody()) cloneAncestor = cloneRigidBody(originalAncestor, chainRootFrame, cloneSuffix, null, null); else if (chainRootFrame != null) cloneAncestor = new RigidBody(originalAncestor.getName() + cloneSuffix, chainRootFrame); JointBasics cloneJoint = cloneJoint(originalJoint, cloneSuffix, clonePredecessor, false, null); RigidBodyBasics cloneSuccessor = cloneRigidBody(originalSuccessor, null, cloneSuffix, cloneJoint, null); originalToCloneBodyMap.put(originalSuccessor, cloneSuccessor);
/** * Performs a deep copy of the kinematic chain described by the given {@code start} and {@code end}, * then filters the cloned joint to return only the ones implementing {@code OneDoFJoint}. * <p> * The clone of the kinematic chain has its own root body which reference frame is child of the * frame after the parent joint of {@code start}. As a result, the clone is an independent * multi-body system but its root is following the original multi-body system. * </p> * * @param start the rigid-body from where the kinematic chain starts. * @param end the rigid-body where the kinematic chain ends. * @return the clone kinematic chain. */ public static OneDoFJointBasics[] cloneOneDoFJointKinematicChain(RigidBodyBasics start, RigidBodyBasics end) { return cloneKinematicChainAndFilter(MultiBodySystemTools.createOneDoFJointPath(start, end), OneDoFJointBasics.class); }
/** * Performs a deep copy of the given {@code originalJoints}. * <p> * The {@code originalJoints} must represent a continuous kinematic chain. The joints must be stored * in order starting from the joint that is the closest to the root, to end with the joint the * closest to an end-effector. * </p> * <p> * The clone of the kinematic chain has its own root body which reference frame is child of the * frame after the parent joint of {@code originalJoints[0]}. As a result, the clone is an * independent multi-body system but its root is following the original multi-body system. * </p> * * @param originalJoints the kinematic chain to clone. Not modified. * @param cloneSuffix suffix to append to the cloned joints and rigid-bodies. * @return the clone kinematic chain. */ public static JointBasics[] cloneKinematicChain(JointReadOnly[] originalJoints, String cloneSuffix) { return cloneKinematicChain(originalJoints, cloneSuffix, null); }
List<OneDoFJointBasics> jointsInFuture = Arrays.asList(MultiBodySystemFactories.cloneOneDoFJointKinematicChain(joints.toArray(new OneDoFJointBasics[numberOfJoints])));
/** * Performs a deep copy of {@code original}, preserving naming, root body, and the joints to ignore. * The clone is attached to the given {@code clonerootFrame}. * * @param original the multi-body system to clone. Not modified. * @param cloneRootFrame the root frame to which the clone system is attached. * @return the clone. */ public static MultiBodySystemBasics clone(MultiBodySystemReadOnly original, ReferenceFrame cloneRootFrame) { RigidBodyBasics cloneRootBody = MultiBodySystemFactories.cloneMultiBodySystem(original.getRootBody(), cloneRootFrame, ""); Set<String> namesOfJointsToConsider = SubtreeStreams.fromChildren(original.getRootBody()).map(JointReadOnly::getName).collect(Collectors.toSet()); List<? extends JointBasics> jointsToConsider = SubtreeStreams.fromChildren(cloneRootBody) .filter(joint -> namesOfJointsToConsider.contains(joint.getName())) .collect(Collectors.toList()); return toMultiBodySystemBasics(jointsToConsider); } }
/** * Performs a deep copy of the given {@code originalJoints} then filters the cloned joint to return * only the ones that implement the given {@code clazz}. * <p> * The {@code originalJoints} must represent a continuous kinematic chain. The joints must be stored * in order starting from the joint that is the closest to the root, to end with the joint the * closest to an end-effector. * </p> * <p> * The clone of the kinematic chain has its own root body which reference frame is child of the * frame after the parent joint of {@code originalJoints[0]}. As a result, the clone is an * independent multi-body system but its root is following the original multi-body system. * </p> * * @param originalJoints the kinematic chain to clone. Not modified. * @param clazz class used to filter the cloned joints that are to be returned. * @return the clone kinematic chain. */ public static <T extends JointReadOnly> T[] cloneKinematicChainAndFilter(T[] originalJoints, Class<T> clazz) { return MultiBodySystemTools.filterJoints(cloneKinematicChain(originalJoints), clazz); }
private ChestTrajectoryMessage createRandomChestMessage(double trajectoryTime, Random random) { OneDoFJointBasics[] spineClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(pelvis, chest); MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, spineClone); RigidBodyBasics chestClone = spineClone[spineClone.length - 1].getSuccessor(); FrameQuaternion desiredRandomChestOrientation = new FrameQuaternion(chestClone.getBodyFixedFrame()); desiredRandomChestOrientation.changeFrame(ReferenceFrame.getWorldFrame()); Quaternion desiredOrientation = new Quaternion(desiredRandomChestOrientation); return HumanoidMessageTools.createChestTrajectoryMessage(trajectoryTime, desiredOrientation, ReferenceFrame.getWorldFrame(), ReferenceFrame.getWorldFrame()); }
List<OneDoFJoint> joints = MultiBodySystemRandomTools.nextOneDoFJointTree(random, numberOfJoints); RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(joints.get(0).getPredecessor()); RigidBodyBasics rootBodyInFuture = MultiBodySystemFactories.cloneMultiBodySystem(rootBody, worldFrame, "Test"); List<OneDoFJointBasics> jointsInFuture = SubtreeStreams.fromChildren(OneDoFJointBasics.class, rootBodyInFuture).collect(Collectors.toList());
/** * Performs a deep copy of the given {@code originalJoints} then filters the cloned joint to return * only the ones that implement the given {@code clazz}. * <p> * The {@code originalJoints} must represent a continuous kinematic chain. The joints must be stored * in order starting from the joint that is the closest to the root, to end with the joint the * closest to an end-effector. * </p> * <p> * The clone of the kinematic chain has its own root body which reference frame is child of the * frame after the parent joint of {@code originalJoints[0]}. As a result, the clone is an * independent multi-body system but its root is following the original multi-body system. * </p> * * @param originalJoints the kinematic chain to clone. Not modified. * @param clazz class used to filter the cloned joints that are to be returned. * @param cloneSuffix suffix to append to the cloned joints and rigid-bodies. * @return the clone kinematic chain. */ public static <T extends JointReadOnly> T[] cloneKinematicChainAndFilter(T[] originalJoints, Class<T> clazz, String cloneSuffix) { return MultiBodySystemTools.filterJoints(cloneKinematicChain(originalJoints, cloneSuffix), clazz); }
RigidBodyBasics chest = fullRobotModel.getChest(); OneDoFJointBasics[] spineClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(pelvis, chest); MultiBodySystemRandomTools.nextStateWithinJointLimits(random, JointStateType.CONFIGURATION, spineClone); RigidBodyBasics chestClone = spineClone[spineClone.length - 1].getSuccessor();
public static NumericalInverseKinematicsCalculator createIKCalculator(JointBasics[] jointsToControl, int maxIterations) { JointBasics[] cloneOfControlledJoints = MultiBodySystemFactories.cloneKinematicChain(jointsToControl); int numberOfDoFs = cloneOfControlledJoints.length; RigidBodyBasics cloneOfEndEffector = cloneOfControlledJoints[numberOfDoFs - 1].getSuccessor(); ReferenceFrame cloneOfEndEffectorFrame = cloneOfEndEffector.getBodyFixedFrame(); GeometricJacobian jacobian = new GeometricJacobian(cloneOfControlledJoints, cloneOfEndEffectorFrame); double lambdaLeastSquares = 0.0009; double tolerance = 0.001; double maxStepSize = 0.2; double minRandomSearchScalar = 0.02; double maxRandomSearchScalar = 0.8; return new NumericalInverseKinematicsCalculator(jacobian, lambdaLeastSquares, tolerance, maxIterations, maxStepSize, minRandomSearchScalar, maxRandomSearchScalar); }
RigidBodyBasics chest = fullRobotModel.getChest(); OneDoFJointBasics[] spineClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(pelvis, chest);
List<RevoluteJoint> revoluteJoints = floatingChain.getRevoluteJoints(); List<Joint> joints = floatingChain.getJoints(); List<JointBasics> jointsInFuture = Arrays.asList(MultiBodySystemFactories.cloneKinematicChain(joints.toArray(new JointBasics[numberOfRevoluteJoints + 1]))); SixDoFJoint floatingJointInFuture = (SixDoFJoint) jointsInFuture.get(0);
OneDoFJointBasics[] spineClone = MultiBodySystemFactories.cloneOneDoFJointKinematicChain(pelvis, chest);