/** * 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); }
/** * 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 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); }
/** * 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); }
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); }
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);
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);
upperArmJointsClone.put(robotSide, MultiBodySystemTools.filterJoints(MultiBodySystemFactories.cloneKinematicChain(upperArmJoints.get(robotSide)), OneDoFJointBasics.class)); GeometricJacobian upperArmJacobian = new GeometricJacobian(upperArmJointsClone.get(robotSide), upperArmJointsClone.get(robotSide)[upperArmJointsClone.get(robotSide).length lowerArmJointsClone.put(robotSide, MultiBodySystemTools.filterJoints(MultiBodySystemFactories.cloneKinematicChain(lowerArmJoints.get(robotSide)), OneDoFJointBasics.class)); GeometricJacobian lowerArmJacobian = new GeometricJacobian(lowerArmJointsClone.get(robotSide), lowerArmJointsClone.get(robotSide)[lowerArmJointsClone.get(robotSide).length