Best Java code snippets using us.ihmc.mecano.tools.MultiBodySystemFactories.cloneRigidBody (Showing top 3 results out of 315)

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 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.
* @param rigidBodyBuilder the builder to use for creating rigid-bodies. If {@code null},
*           {@link #DEFAULT_RIGID_BODY_BUILDER} is used.
* @param jointBuilder the builder to use for creating joints. If {@code null},
*           {@link #DEFAULT_JOINT_BUILDER} is used.
* @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,
                         RigidBodyBuilder rigidBodyBuilder, JointBuilder jointBuilder)
{
 if (!originalRootBody.isRootBody())
   throw new IllegalArgumentException("The given rigid-body is not the root-body of its multi-body system: " + originalRootBody.getName());
 RigidBodyBasics cloneSubtreeStartBody = cloneRigidBody(originalRootBody, cloneStationaryFrame, cloneSuffix, null, rigidBodyBuilder);
 cloneSubtree(originalRootBody, cloneSubtreeStartBody, cloneSuffix, rigidBodyBuilder, jointBuilder);
 return cloneSubtreeStartBody;
}

RigidBodyBasics cloneAncestor;
if (originalAncestor.isRootBody())
  cloneAncestor = cloneRigidBody(originalAncestor, chainRootFrame, cloneSuffix, null, null);
else if (chainRootFrame != null)
  cloneAncestor = new RigidBody(originalAncestor.getName() + cloneSuffix, chainRootFrame);
  RigidBodyBasics cloneSuccessor = cloneRigidBody(originalSuccessor, null, cloneSuffix, cloneJoint, null);
  originalToCloneBodyMap.put(originalSuccessor, cloneSuccessor);