All Downloads are FREE. Search and download functionalities are using the official Maven repository.

us.ihmc.scs2.definition.robot.ValkyrieModelLoadingTest Maven / Gradle / Ivy

There is a newer version: 17-0.28.3
Show newest version
package us.ihmc.scs2.definition.robot;

import org.junit.jupiter.api.Test;
import us.ihmc.euclid.matrix.Matrix3D;
import us.ihmc.euclid.matrix.interfaces.Matrix3DReadOnly;
import us.ihmc.euclid.orientation.interfaces.Orientation3DBasics;
import us.ihmc.euclid.tools.EuclidCoreTestTools;
import us.ihmc.euclid.tools.EuclidCoreTools;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
import us.ihmc.scs2.definition.DefinitionIOTools;
import us.ihmc.scs2.definition.collision.CollisionShapeDefinition;
import us.ihmc.scs2.definition.robot.sdf.SDFTools;
import us.ihmc.scs2.definition.robot.sdf.items.SDFRoot;
import us.ihmc.scs2.definition.robot.urdf.URDFTools;
import us.ihmc.scs2.definition.robot.urdf.items.URDFModel;
import us.ihmc.scs2.definition.visual.VisualDefinition;

import javax.xml.bind.JAXBException;
import java.io.*;
import java.lang.reflect.Array;
import java.util.*;
import java.util.stream.Stream;

import static org.junit.jupiter.api.Assertions.*;
import static us.ihmc.euclid.tools.EuclidCoreTestTools.addPrefixToMessage;
import static us.ihmc.euclid.tools.EuclidCoreTestTools.throwNotEqualAssertionError;

public class ValkyrieModelLoadingTest
{
   private static final double EPSILON = 1.0e-5;

   private static final String LeftHipYawName = "leftHipYaw";
   private static final String LeftHipRollName = "leftHipRoll";
   private static final String LeftHipPitchName = "leftHipPitch";
   private static final String LeftKneePitchName = "leftKneePitch";
   private static final String LeftAnklePitchName = "leftAnklePitch";
   private static final String LeftAnkleRollName = "leftAnkleRoll";

   private static final String RightHipYawName = "rightHipYaw";
   private static final String RightHipRollName = "rightHipRoll";
   private static final String RightHipPitchName = "rightHipPitch";
   private static final String RightKneePitchName = "rightKneePitch";
   private static final String RightAnklePitchName = "rightAnklePitch";
   private static final String RightAnkleRollName = "rightAnkleRoll";

   private static final String TorsoYawName = "torsoYaw";
   private static final String TorsoPitchName = "torsoPitch";
   private static final String TorsoRollName = "torsoRoll";

   private static final String NeckLowerPitchName = "lowerNeckPitch";
   private static final String NeckYawName = "neckYaw";
   private static final String NeckUpperPitchName = "upperNeckPitch";

   private static final String HokuyoJointName = "hokuyo_joint";

   private static final String LeftShoulderPitchName = "leftShoulderPitch";
   private static final String LeftShoulderRollName = "leftShoulderRoll";
   private static final String LeftShoulderYawName = "leftShoulderYaw";
   private static final String LeftElbowPitchName = "leftElbowPitch";
   private static final String LeftForearmYawName = "leftForearmYaw";
   private static final String LeftWristPitchName = "leftWristPitch";
   private static final String LeftWristRollName = "leftWristRoll";

   private static final String RightShoulderPitchName = "rightShoulderPitch";
   private static final String RightShoulderRollName = "rightShoulderRoll";
   private static final String RightShoulderYawName = "rightShoulderYaw";
   private static final String RightElbowPitchName = "rightElbowPitch";
   private static final String RightForearmYawName = "rightForearmYaw";
   private static final String RightWristPitchName = "rightWristPitch";
   private static final String RightWristRollName = "rightWristRoll";

   private static final String PelvisName = "pelvis";

   private static final String LeftHipYawLinkName = "leftHipYawLink";
   private static final String LeftHipRollLinkName = "leftHipRollLink";
   private static final String LeftHipPitchLinkName = "leftHipPitchLink";
   private static final String LeftKneePitchLinkName = "leftKneePitchLink";
   private static final String LeftAnklePitchLinkName = "leftAnklePitchLink";
   private static final String LeftFootName = "leftFoot";

   private static final String RightHipYawLinkName = "rightHipYawLink";
   private static final String RightHipRollLinkName = "rightHipRollLink";
   private static final String RightHipPitchLinkName = "rightHipPitchLink";
   private static final String RightKneePitchLinkName = "rightKneePitchLink";
   private static final String RightAnklePitchLinkName = "rightAnklePitchLink";
   private static final String RightFootName = "rightFoot";

   private static final String TorsoYawLinkName = "torsoYawLink";
   private static final String TorsoPitchLinkName = "torsoPitchLink";
   private static final String TorsoName = "torso";

   private static final String NeckLowerPitchLinkName = "lowerNeckPitchLink";
   private static final String NeckYawLinkName = "neckYawLink";
   private static final String NeckUpperPitchLinkName = "upperNeckPitchLink";

   private static final String HokuyoLinkName = "hokuyo_link";

   private static final String LeftShoulderPitchLinkName = "leftShoulderPitchLink";
   private static final String LeftShoulderRollLinkName = "leftShoulderRollLink";
   private static final String LeftShoulderYawLinkName = "leftShoulderYawLink";
   private static final String LeftElbowPitchLinkName = "leftElbowPitchLink";
   private static final String LeftForearmLinkName = "leftForearmLink";
   private static final String LeftWristRollLinkName = "leftWristRollLink";
   private static final String LeftPalmName = "leftPalm";

   private static final String RightShoulderPitchLinkName = "rightShoulderPitchLink";
   private static final String RightShoulderRollLinkName = "rightShoulderRollLink";
   private static final String RightShoulderYawLinkName = "rightShoulderYawLink";
   private static final String RightElbowPitchLinkName = "rightElbowPitchLink";
   private static final String RightForearmLinkName = "rightForearmLink";
   private static final String RightWristRollLinkName = "rightWristRollLink";
   private static final String RightPalmName = "rightPalm";

   private static final String[] LeftLegJointNames = {LeftHipYawName,
                                                      LeftHipRollName,
                                                      LeftHipPitchName,
                                                      LeftKneePitchName,
                                                      LeftAnklePitchName,
                                                      LeftAnkleRollName};
   private static final String[] RightLegJointNames = {RightHipYawName,
                                                       RightHipRollName,
                                                       RightHipPitchName,
                                                       RightKneePitchName,
                                                       RightAnklePitchName,
                                                       RightAnkleRollName};
   private static final String[] TorsoJointNames = {TorsoYawName, TorsoPitchName, TorsoRollName};
   private static final String[] NeckJointNames = {NeckLowerPitchName, NeckYawName, NeckUpperPitchName};
   private static final String[] LeftArmJointNames = {LeftShoulderPitchName,
                                                      LeftShoulderRollName,
                                                      LeftShoulderYawName,
                                                      LeftElbowPitchName,
                                                      LeftForearmYawName,
                                                      LeftWristRollName,
                                                      LeftWristPitchName};
   private static final String[] LeftIndexFingerJointNames = {"leftIndexFingerPitch1", "leftIndexFingerPitch2", "leftIndexFingerPitch3"};
   private static final String[] LeftMiddleFingerJointNames = {"leftMiddleFingerPitch1", "leftMiddleFingerPitch2", "leftMiddleFingerPitch3"};
   private static final String[] LeftPinkyJointNames = {"leftPinkyPitch1", "leftPinkyPitch2", "leftPinkyPitch3"};
   private static final String[] LeftThumbJointNames = {"leftThumbRoll", "leftThumbPitch1", "leftThumbPitch2", "leftThumbPitch3"};
   private static final String[] RightArmJointNames = {RightShoulderPitchName,
                                                       RightShoulderRollName,
                                                       RightShoulderYawName,
                                                       RightElbowPitchName,
                                                       RightForearmYawName,
                                                       RightWristRollName,
                                                       RightWristPitchName};
   private static final String[] RightIndexFingerJointNames = {"rightIndexFingerPitch1", "rightIndexFingerPitch2", "rightIndexFingerPitch3"};
   private static final String[] RightMiddleFingerJointNames = {"rightMiddleFingerPitch1", "rightMiddleFingerPitch2", "rightMiddleFingerPitch3"};
   private static final String[] RightPinkyJointNames = {"rightPinkyPitch1", "rightPinkyPitch2", "rightPinkyPitch3"};
   private static final String[] RightThumbJointNames = {"rightThumbRoll", "rightThumbPitch1", "rightThumbPitch2", "rightThumbPitch3"};
   private static final String[] AllJointNames = concatenate(new String[] {PelvisName, HokuyoJointName},
                                                             LeftLegJointNames,
                                                             RightLegJointNames,
                                                             TorsoJointNames,
                                                             NeckJointNames,
                                                             LeftArmJointNames,
                                                             LeftIndexFingerJointNames,
                                                             LeftMiddleFingerJointNames,
                                                             LeftPinkyJointNames,
                                                             LeftThumbJointNames,
                                                             RightArmJointNames,
                                                             RightIndexFingerJointNames,
                                                             RightMiddleFingerJointNames,
                                                             RightPinkyJointNames,
                                                             RightThumbJointNames);

   private static final String[] LeftLegLinkNames = {LeftHipYawLinkName,
                                                     LeftHipRollLinkName,
                                                     LeftHipPitchLinkName,
                                                     LeftKneePitchLinkName,
                                                     LeftAnklePitchLinkName,
                                                     LeftFootName};
   private static final String[] RightLegLinkNames = {RightHipYawLinkName,
                                                      RightHipRollLinkName,
                                                      RightHipPitchLinkName,
                                                      RightKneePitchLinkName,
                                                      RightAnklePitchLinkName,
                                                      RightFootName};
   private static final String[] TorsoLinkNames = {TorsoYawLinkName, TorsoPitchLinkName, TorsoName};
   private static final String[] NeckLinkNames = {NeckLowerPitchLinkName, NeckYawLinkName, NeckUpperPitchLinkName};
   private static final String[] LeftArmLinkNames = {LeftShoulderPitchLinkName,
                                                     LeftShoulderRollLinkName,
                                                     LeftShoulderYawLinkName,
                                                     LeftElbowPitchLinkName,
                                                     LeftForearmLinkName,
                                                     LeftWristRollLinkName,
                                                     LeftPalmName};
   private static final String[] LeftIndexFingerLinkNames = Stream.of(LeftIndexFingerJointNames).map(s -> s + "Link").toArray(String[]::new);
   private static final String[] LeftMiddleFingerLinkNames = Stream.of(LeftMiddleFingerJointNames).map(s -> s + "Link").toArray(String[]::new);
   private static final String[] LeftPinkyLinkNames = Stream.of(LeftPinkyJointNames).map(s -> s + "Link").toArray(String[]::new);
   private static final String[] LeftThumbLinkNames = Stream.of(LeftThumbJointNames).map(s -> s + "Link").toArray(String[]::new);
   private static final String[] RightArmLinkNames = {RightShoulderPitchLinkName,
                                                      RightShoulderRollLinkName,
                                                      RightShoulderYawLinkName,
                                                      RightElbowPitchLinkName,
                                                      RightForearmLinkName,
                                                      RightWristRollLinkName,
                                                      RightPalmName};
   private static final String[] RightIndexFingerLinkNames = Stream.of(RightIndexFingerJointNames).map(s -> s + "Link").toArray(String[]::new);
   private static final String[] RightMiddleFingerLinkNames = Stream.of(RightMiddleFingerJointNames).map(s -> s + "Link").toArray(String[]::new);
   private static final String[] RightPinkyLinkNames = Stream.of(RightPinkyJointNames).map(s -> s + "Link").toArray(String[]::new);
   private static final String[] RightThumbLinkNames = Stream.of(RightThumbJointNames).map(s -> s + "Link").toArray(String[]::new);
   private static final String[] AllLinkNames = concatenate(new String[] {PelvisName, HokuyoLinkName},
                                                            LeftLegLinkNames,
                                                            RightLegLinkNames,
                                                            TorsoLinkNames,
                                                            NeckLinkNames,
                                                            LeftArmLinkNames,
                                                            LeftIndexFingerLinkNames,
                                                            LeftMiddleFingerLinkNames,
                                                            LeftPinkyLinkNames,
                                                            LeftThumbLinkNames,
                                                            RightArmLinkNames,
                                                            RightIndexFingerLinkNames,
                                                            RightMiddleFingerLinkNames,
                                                            RightPinkyLinkNames,
                                                            RightThumbLinkNames);

   @Test
   public void testSDFTools() throws Exception
   {
      InputStream resourceAsStream = this.getClass().getClassLoader().getResourceAsStream("models/valkyrie/valkyrie_sim.sdf");
      SDFRoot sdfRoot = SDFTools.loadSDFRoot(resourceAsStream, Collections.emptyList(), this.getClass().getClassLoader());
      RobotDefinition robotDefinition = SDFTools.toFloatingRobotDefinition(sdfRoot.getModels().get(0));
      robotDefinition.transformAllFramesToZUp();
      performAssertionsOnRobotDefinition(robotDefinition);
   }

   @Test
   public void testURDFTools() throws Exception
   {
      InputStream resourceAsStream = this.getClass().getClassLoader().getResourceAsStream("models/valkyrie/valkyrie_sim.urdf");
      URDFModel urdfModel = URDFTools.loadURDFModel(resourceAsStream, Collections.emptyList(), this.getClass().getClassLoader());
      RobotDefinition robotDefinition = URDFTools.toRobotDefinition(urdfModel);
      performAssertionsOnRobotDefinition(robotDefinition);
   }

   @Test
   public void testSDF_vs_URDFTools() throws Exception
   {
      InputStream resourceAsStream;
      resourceAsStream = this.getClass().getClassLoader().getResourceAsStream("models/valkyrie/valkyrie_sim.sdf");
      SDFRoot sdfRoot = SDFTools.loadSDFRoot(resourceAsStream, Collections.emptyList(), this.getClass().getClassLoader());
      RobotDefinition robotSDF = SDFTools.toFloatingRobotDefinition(sdfRoot.getModels().get(0));

      resourceAsStream = this.getClass().getClassLoader().getResourceAsStream("models/valkyrie/valkyrie_sim.urdf");
      URDFModel urdfModel = URDFTools.loadURDFModel(resourceAsStream, Collections.emptyList(), this.getClass().getClassLoader());
      RobotDefinition robotURDF = URDFTools.toRobotDefinition(urdfModel);

      for (RigidBodyDefinition bodySDF : robotSDF.getAllRigidBodies())
      {
         RigidBodyDefinition bodyURDF = robotSDF.getRigidBodyDefinition(bodySDF.getName());
         assertNotNull(bodyURDF, "Couldn't find the rigid-body %s in the URDF robot definition.".formatted(bodySDF.getName()));
         assertRigidBodyEquals(bodySDF, bodyURDF, EPSILON);
      }

      for (JointDefinition jointSDF : robotSDF.getAllJoints())
      {
         if (jointSDF.getName().equals(HokuyoJointName))
            continue; // Something wonky about that guy
         JointDefinition jointURDF = robotURDF.getJointDefinition(jointSDF.getName());
         assertNotNull(jointURDF, "Couldn't find the joint %s in the URDF robot definition.".formatted(jointSDF.getName()));
         jointURDF.getSensorDefinitions().removeIf(sensor -> sensor instanceof WrenchSensorDefinition); // SDF doesn't have tags for that sensor
         assertJointEquals(jointSDF, jointURDF, EPSILON);
      }
   }

   public static void assertRigidBodyEquals(RigidBodyDefinition expected, RigidBodyDefinition actual, double epsilon)
   {
      if (!Objects.equals(expected.getName(), actual.getName()))
         throwNotEqualAssertionError("Rigid-bodies do not have the same name", expected.getName(), actual.getName());
      if (!EuclidCoreTools.equals(expected.getMass(), actual.getMass()))
         throwNotEqualAssertionError("Rigid-bodies (%s) do not have the same mass".formatted(expected.getName()),
                                     Double.toString(expected.getMass()),
                                     Double.toString(actual.getMass()));
      if (!EuclidCoreTools.epsilonEquals(expected.getMomentOfInertia(), actual.getMomentOfInertia(), epsilon))
         throwNotEqualAssertionError("Rigid-bodies (%s) do not have the same MoI".formatted(expected.getName()),
                                     Objects.toString(expected.getMass()),
                                     Objects.toString(actual.getMass()));
      if (!EuclidCoreTools.epsilonEquals(expected.getInertiaPose(), actual.getInertiaPose(), epsilon))
         throwNotEqualAssertionError("Rigid-bodies (%s) do not have the same inertia pose".formatted(expected.getName()),
                                     Objects.toString(expected.getInertiaPose()),
                                     Objects.toString(actual.getInertiaPose()));
      if (!Objects.equals(expected.getChildrenJoints().size(), actual.getChildrenJoints().size()))
         throwNotEqualAssertionError("Rigid-bodies (%s) do not have the same number of children".formatted(expected.getName()),
                                     Integer.toString(expected.getChildrenJoints().size()),
                                     Integer.toString(actual.getChildrenJoints().size()));
      if (!Objects.equals(expected.getVisualDefinitions().size(), actual.getVisualDefinitions().size()))
         throwNotEqualAssertionError("Rigid-bodies (%s) do not have the same number of visuals".formatted(expected.getName()),
                                     Integer.toString(expected.getVisualDefinitions().size()),
                                     Integer.toString(actual.getVisualDefinitions().size()));

      for (int i = 0; i < expected.getVisualDefinitions().size(); i++)
      {
         VisualDefinition expectedVisual = expected.getVisualDefinitions().get(i);
         VisualDefinition actualVisual = actual.getVisualDefinitions().get(i);
         assertVisualEquals("In rigid-body (%s)".formatted(expected.getName()), expectedVisual, actualVisual, epsilon);
      }

      if (!Objects.equals(expected.getCollisionShapeDefinitions().size(), actual.getCollisionShapeDefinitions().size()))
         throwNotEqualAssertionError("Rigid-bodies (%s) do not have the same number of collisions".formatted(expected.getName()),
                                     Integer.toString(expected.getCollisionShapeDefinitions().size()),
                                     Integer.toString(actual.getCollisionShapeDefinitions().size()));

      for (int i = 0; i < expected.getCollisionShapeDefinitions().size(); i++)
      {
         CollisionShapeDefinition expectedCollisionShape = expected.getCollisionShapeDefinitions().get(i);
         CollisionShapeDefinition actualCollisionShape = actual.getCollisionShapeDefinitions().get(i);
         assertCollisionShapeEquals("In rigid-body (%s)".formatted(expected.getName()), expectedCollisionShape, actualCollisionShape, epsilon);
      }
   }

   public static void assertVisualEquals(String messagePrefix, VisualDefinition expected, VisualDefinition actual, double epsilon)
   {
      if (!Objects.equals(expected.getName(), actual.getName()))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Visuals do not have the same name"), expected.getName(), actual.getName());
      if (!EuclidCoreTools.epsilonEquals(expected.getOriginPose(), actual.getOriginPose(), epsilon))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Visuals (%s) do not have the same pose".formatted(expected.getName())),
                                     Objects.toString(expected.getOriginPose()),
                                     Objects.toString(actual.getOriginPose()));
      if (!Objects.equals(expected.getGeometryDefinition(), actual.getGeometryDefinition()))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Visuals (%s) do not have the same geometry".formatted(expected.getName())),
                                     Objects.toString(expected.getGeometryDefinition()),
                                     Objects.toString(actual.getGeometryDefinition()));
      if (!Objects.equals(expected.getMaterialDefinition(), actual.getMaterialDefinition()))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Visuals (%s) do not have the same material".formatted(expected.getName())),
                                     Objects.toString(expected.getMaterialDefinition()),
                                     Objects.toString(actual.getMaterialDefinition()));
   }

   public static void assertCollisionShapeEquals(String messagePrefix, CollisionShapeDefinition expected, CollisionShapeDefinition actual, double epsilon)
   {
      if (!Objects.equals(expected.getName(), actual.getName()))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Collisions do not have the same name"), expected.getName(), actual.getName());
      if (!EuclidCoreTools.epsilonEquals(expected.getOriginPose(), actual.getOriginPose(), epsilon))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Collisions (%s) do not have the same pose".formatted(expected.getName())),
                                     Objects.toString(expected.getOriginPose()),
                                     Objects.toString(actual.getOriginPose()));
      if (!Objects.equals(expected.getGeometryDefinition(), actual.getGeometryDefinition()))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Collisions (%s) do not have the same geometry".formatted(expected.getName())),
                                     Objects.toString(expected.getGeometryDefinition()),
                                     Objects.toString(actual.getGeometryDefinition()));
      if (expected.isConcave() != actual.isConcave())
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Collisions (%s) do not have the concave property".formatted(expected.getName())),
                                     Objects.toString(expected.isConcave()),
                                     Objects.toString(actual.isConcave()));

      if (expected.getCollisionMask() != actual.getCollisionMask())
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Collisions (%s) do not have the collision mask".formatted(expected.getName())),
                                     Objects.toString(expected.getCollisionMask()),
                                     Objects.toString(actual.getCollisionMask()));

      if (expected.getCollisionGroup() != actual.getCollisionGroup())
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Collisions (%s) do not have the collision group".formatted(expected.getName())),
                                     Objects.toString(expected.getCollisionGroup()),
                                     Objects.toString(actual.getCollisionGroup()));
   }

   public static void assertJointEquals(JointDefinition expected, JointDefinition actual, double epsilon)
   {

      if (!Objects.equals(expected.getName(), actual.getName()))
         throwNotEqualAssertionError("Joints do not have the same name", expected.getName(), actual.getName());
      if (!EuclidCoreTools.epsilonEquals(expected.getTransformToParent(), actual.getTransformToParent(), epsilon))
         throwNotEqualAssertionError("Joints (%s) do not have the same transform".formatted(expected.getName()),
                                     Objects.toString(expected.getTransformToParent()),
                                     Objects.toString(actual.getTransformToParent()));

      if (expected instanceof OneDoFJointDefinition expectedOneDoFJoint)
      {
         OneDoFJointDefinition actualOneDoFJoint = (OneDoFJointDefinition) actual;

         if (!EuclidCoreTools.epsilonEquals(expectedOneDoFJoint.getAxis(), actualOneDoFJoint.getAxis(), epsilon))
            throwNotEqualAssertionError("Joints (%s) do not have the same axis".formatted(expected.getName()),
                                        Objects.toString(expectedOneDoFJoint.getAxis()),
                                        Objects.toString(actualOneDoFJoint.getAxis()));
         if (!EuclidCoreTools.epsilonEquals(expectedOneDoFJoint.getPositionLowerLimit(), actualOneDoFJoint.getPositionLowerLimit(), epsilon))
            throwNotEqualAssertionError("Joints (%s) do not have the same position lower limit".formatted(expected.getName()),
                                        Double.toString(expectedOneDoFJoint.getPositionLowerLimit()),
                                        Double.toString(actualOneDoFJoint.getPositionLowerLimit()));
         if (!EuclidCoreTools.epsilonEquals(expectedOneDoFJoint.getPositionUpperLimit(), actualOneDoFJoint.getPositionUpperLimit(), epsilon))
            throwNotEqualAssertionError("Joints (%s) do not have the same position upper limit".formatted(expected.getName()),
                                        Double.toString(expectedOneDoFJoint.getPositionUpperLimit()),
                                        Double.toString(actualOneDoFJoint.getPositionUpperLimit()));
         if (!EuclidCoreTools.epsilonEquals(expectedOneDoFJoint.getVelocityLowerLimit(), actualOneDoFJoint.getVelocityLowerLimit(), epsilon))
            throwNotEqualAssertionError("Joints (%s) do not have the same velocity lower limit".formatted(expected.getName()),
                                        Double.toString(expectedOneDoFJoint.getVelocityLowerLimit()),
                                        Double.toString(actualOneDoFJoint.getVelocityLowerLimit()));
         if (!EuclidCoreTools.epsilonEquals(expectedOneDoFJoint.getVelocityUpperLimit(), actualOneDoFJoint.getVelocityUpperLimit(), epsilon))
            throwNotEqualAssertionError("Joints (%s) do not have the same velocity upper limit".formatted(expected.getName()),
                                        Double.toString(expectedOneDoFJoint.getVelocityUpperLimit()),
                                        Double.toString(actualOneDoFJoint.getVelocityUpperLimit()));
         if (!EuclidCoreTools.epsilonEquals(expectedOneDoFJoint.getEffortLowerLimit(), actualOneDoFJoint.getEffortLowerLimit(), epsilon))
            throwNotEqualAssertionError("Joints (%s) do not have the same effort lower limit".formatted(expected.getName()),
                                        Double.toString(expectedOneDoFJoint.getEffortLowerLimit()),
                                        Double.toString(actualOneDoFJoint.getEffortLowerLimit()));
         if (!EuclidCoreTools.epsilonEquals(expectedOneDoFJoint.getEffortUpperLimit(), actualOneDoFJoint.getEffortUpperLimit(), epsilon))
            throwNotEqualAssertionError("Joints (%s) do not have the same effort upper limit".formatted(expected.getName()),
                                        Double.toString(expectedOneDoFJoint.getEffortUpperLimit()),
                                        Double.toString(actualOneDoFJoint.getEffortUpperLimit()));
         if (!EuclidCoreTools.epsilonEquals(expectedOneDoFJoint.getDamping(), actualOneDoFJoint.getDamping(), epsilon))
            throwNotEqualAssertionError("Joints (%s) do not have the same damping".formatted(expected.getName()),
                                        Double.toString(expectedOneDoFJoint.getDamping()),
                                        Double.toString(actualOneDoFJoint.getDamping()));
         if (!EuclidCoreTools.epsilonEquals(expectedOneDoFJoint.getStiction(), actualOneDoFJoint.getStiction(), epsilon))
            throwNotEqualAssertionError("Joints (%s) do not have the same stiction".formatted(expected.getName()),
                                        Double.toString(expectedOneDoFJoint.getStiction()),
                                        Double.toString(actualOneDoFJoint.getStiction()));
         if (!EuclidCoreTools.epsilonEquals(expectedOneDoFJoint.getKpSoftLimitStop(), actualOneDoFJoint.getKpSoftLimitStop(), epsilon))
            throwNotEqualAssertionError("Joints (%s) do not have the same kp soft limit stop".formatted(expected.getName()),
                                        Double.toString(expectedOneDoFJoint.getKpSoftLimitStop()),
                                        Double.toString(actualOneDoFJoint.getKpSoftLimitStop()));
         if (!EuclidCoreTools.epsilonEquals(expectedOneDoFJoint.getKdSoftLimitStop(), actualOneDoFJoint.getKdSoftLimitStop(), epsilon))
            throwNotEqualAssertionError("Joints (%s) do not have the same kd soft limit stop".formatted(expected.getName()),
                                        Double.toString(expectedOneDoFJoint.getKdSoftLimitStop()),
                                        Double.toString(actualOneDoFJoint.getKdSoftLimitStop()));
         if (!EuclidCoreTools.epsilonEquals(expectedOneDoFJoint.getDampingVelocitySoftLimit(), actualOneDoFJoint.getDampingVelocitySoftLimit(), epsilon))
            throwNotEqualAssertionError("Joints (%s) do not have the same damping velocity soft limit".formatted(expected.getName()),
                                        Double.toString(expectedOneDoFJoint.getDampingVelocitySoftLimit()),
                                        Double.toString(actualOneDoFJoint.getDampingVelocitySoftLimit()));
      }

      if (!Objects.equals(expected.getSensorDefinitions().size(), actual.getSensorDefinitions().size()))
         throwNotEqualAssertionError("Joints (%s) do not have the same number of sensors".formatted(expected.getName()),
                                     Objects.toString(expected.getSensorDefinitions().size()),
                                     Objects.toString(actual.getSensorDefinitions().size()));

      Collections.sort(expected.getSensorDefinitions(), (s1, s2) -> s1.getName().compareTo(s2.getName()));
      Collections.sort(actual.getSensorDefinitions(), (s1, s2) -> s1.getName().compareTo(s2.getName()));

      for (int i = 0; i < expected.getSensorDefinitions().size(); i++)
      {
         SensorDefinition expectedSensor = expected.getSensorDefinitions().get(i);
         SensorDefinition actualSensor = actual.getSensorDefinitions().get(i);
         assertSensorEquals("In rigid-body (%s)".formatted(expected.getName()), expectedSensor, actualSensor, epsilon);
      }

      if (!Objects.equals(expected.getKinematicPointDefinitions().size(), actual.getKinematicPointDefinitions().size()))
         throwNotEqualAssertionError("Joints (%s) do not have the same number of kinematic points".formatted(expected.getName()),
                                     Objects.toString(expected.getKinematicPointDefinitions().size()),
                                     Objects.toString(actual.getKinematicPointDefinitions().size()));

      for (int i = 0; i < expected.getKinematicPointDefinitions().size(); i++)
      {
         KinematicPointDefinition expectedKinematicPoint = expected.getKinematicPointDefinitions().get(i);
         KinematicPointDefinition actualKinematicPoint = actual.getKinematicPointDefinitions().get(i);
         assertKinematicPointEquals("In rigid-body (%s)".formatted(expected.getName()), expectedKinematicPoint, actualKinematicPoint, epsilon);
      }

      if (!Objects.equals(expected.getExternalWrenchPointDefinitions().size(), actual.getExternalWrenchPointDefinitions().size()))
         throwNotEqualAssertionError("Joints (%s) do not have the same number of external wrench points".formatted(expected.getName()),
                                     Objects.toString(expected.getExternalWrenchPointDefinitions().size()),
                                     Objects.toString(actual.getExternalWrenchPointDefinitions().size()));

      for (int i = 0; i < expected.getExternalWrenchPointDefinitions().size(); i++)
      {
         ExternalWrenchPointDefinition expectedExternalWrenchPoint = expected.getExternalWrenchPointDefinitions().get(i);
         ExternalWrenchPointDefinition actualExternalWrenchPoint = actual.getExternalWrenchPointDefinitions().get(i);
         assertExternalWrenchPointEquals("In rigid-body (%s)".formatted(expected.getName()), expectedExternalWrenchPoint, actualExternalWrenchPoint, epsilon);
      }

      if (!Objects.equals(expected.getGroundContactPointDefinitions().size(), actual.getGroundContactPointDefinitions().size()))
         throwNotEqualAssertionError("Joints (%s) do not have the same number of ground contact points".formatted(expected.getName()),
                                     Objects.toString(expected.getGroundContactPointDefinitions().size()),
                                     Objects.toString(actual.getGroundContactPointDefinitions().size()));

      for (int i = 0; i < expected.getGroundContactPointDefinitions().size(); i++)
      {
         GroundContactPointDefinition expectedGroundContactPoint = expected.getGroundContactPointDefinitions().get(i);
         GroundContactPointDefinition actualGroundContactPoint = actual.getGroundContactPointDefinitions().get(i);
         assertGroundContactPointEquals("In rigid-body (%s)".formatted(expected.getName()), expectedGroundContactPoint, actualGroundContactPoint, epsilon);
      }

      // TODO Skipped the loop closure 
   }

   public static void assertSensorEquals(String messagePrefix, SensorDefinition expected, SensorDefinition actual, double epsilon)
   {
      if (!Objects.equals(expected.getName(), actual.getName()))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Sensors do not have the same name"), expected.getName(), actual.getName());

      if (!EuclidCoreTools.epsilonEquals(expected.getTransformToJoint(), actual.getTransformToJoint(), epsilon))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Sensors (%s) do not have the same pose".formatted(expected.getName())),
                                     Objects.toString(expected.getTransformToJoint()),
                                     Objects.toString(actual.getTransformToJoint()));

      if (expected.getUpdatePeriod() != actual.getUpdatePeriod())
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Sensors (%s) do not have the same udpate period".formatted(expected.getName())),
                                     Objects.toString(expected.getUpdatePeriod()),
                                     Objects.toString(actual.getUpdatePeriod()));

      // TODO Not bothering testing separately all the fields of all the sub-classes
   }

   public static void assertKinematicPointEquals(String messagePrefix, KinematicPointDefinition expected, KinematicPointDefinition actual, double epsilon)
   {
      if (!Objects.equals(expected.getName(), actual.getName()))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Kinematic points do not have the same name"), expected.getName(), actual.getName());

      if (!EuclidCoreTools.epsilonEquals(expected.getTransformToParent(), actual.getTransformToParent(), epsilon))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Kinematic points (%s) do not have the same pose".formatted(expected.getName())),
                                     Objects.toString(expected.getTransformToParent()),
                                     Objects.toString(actual.getTransformToParent()));
   }

   public static void assertExternalWrenchPointEquals(String messagePrefix,
                                                      ExternalWrenchPointDefinition expected,
                                                      ExternalWrenchPointDefinition actual,
                                                      double epsilon)
   {
      if (!Objects.equals(expected.getName(), actual.getName()))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "External wrench points do not have the same name"),
                                     expected.getName(),
                                     actual.getName());

      if (!EuclidCoreTools.epsilonEquals(expected.getTransformToParent(), actual.getTransformToParent(), epsilon))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "External wrench points (%s) do not have the same pose".formatted(expected.getName())),
                                     Objects.toString(expected.getTransformToParent()),
                                     Objects.toString(actual.getTransformToParent()));
   }

   public static void assertGroundContactPointEquals(String messagePrefix,
                                                     GroundContactPointDefinition expected,
                                                     GroundContactPointDefinition actual,
                                                     double epsilon)
   {
      if (!Objects.equals(expected.getName(), actual.getName()))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Ground contact points do not have the same name"),
                                     expected.getName(),
                                     actual.getName());

      if (!EuclidCoreTools.epsilonEquals(expected.getTransformToParent(), actual.getTransformToParent(), epsilon))
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix, "Ground contact points (%s) do not have the same pose".formatted(expected.getName())),
                                     Objects.toString(expected.getTransformToParent()),
                                     Objects.toString(actual.getTransformToParent()));

      if (expected.getGroupIdentifier() != actual.getGroupIdentifier())
         throwNotEqualAssertionError(addPrefixToMessage(messagePrefix,
                                                        "Ground contact points (%s) do not have the same group identifier".formatted(expected.getName())),
                                     Integer.toString(expected.getGroupIdentifier()),
                                     Integer.toString(actual.getGroupIdentifier()));
   }

   @Test
   public void testSaveLoadRobotDefinitionXML() throws JAXBException, FileNotFoundException, IOException
   {
      InputStream resourceAsStream = this.getClass().getClassLoader().getResourceAsStream("models/valkyrie/valkyrie_sim.urdf");
      URDFModel urdfModel = URDFTools.loadURDFModel(resourceAsStream, Collections.emptyList(), this.getClass().getClassLoader());
      RobotDefinition exportedRobotDefinition = URDFTools.toRobotDefinition(urdfModel);
      File testFile = new File("test.xml");
      DefinitionIOTools.saveRobotDefinition(new FileOutputStream(testFile), exportedRobotDefinition);
      RobotDefinition importedRobotDefinition = DefinitionIOTools.loadRobotDefinition(new FileInputStream(testFile));
      performAssertionsOnRobotDefinition(importedRobotDefinition);
      testFile.delete();
   }

   private void performAssertionsOnRobotDefinition(RobotDefinition robotDefinition)
   {
      for (String jointName : AllJointNames)
      {
         assertNotNull(robotDefinition.getJointDefinition(jointName), jointName);
         if (!jointName.equals(PelvisName))
            assertEquals(RevoluteJointDefinition.class, robotDefinition.getJointDefinition(jointName).getClass());
      }

      for (String linkName : AllLinkNames)
      {
         assertNotNull(robotDefinition.getRigidBodyDefinition(linkName), "Link: " + linkName);
      }

      assertEquals(1, robotDefinition.getRootJointDefinitions().size());
      assertEquals(SixDoFJointDefinition.class, robotDefinition.getRootJointDefinitions().get(0).getClass());
      assertEquals(PelvisName, robotDefinition.getRootJointDefinitions().get(0).getName());

      assertKinematicsContinuity(robotDefinition.getRootJointDefinitions().get(0), LeftLegJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getRootJointDefinitions().get(0), RightLegJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getRootJointDefinitions().get(0), TorsoJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getJointDefinition(TorsoRollName), NeckJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getJointDefinition(TorsoRollName), LeftArmJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getJointDefinition(TorsoRollName), RightArmJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getJointDefinition(LeftWristPitchName), LeftIndexFingerJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getJointDefinition(LeftWristPitchName), LeftMiddleFingerJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getJointDefinition(LeftWristPitchName), LeftPinkyJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getJointDefinition(LeftWristPitchName), LeftThumbJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getJointDefinition(RightWristPitchName), RightIndexFingerJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getJointDefinition(RightWristPitchName), RightMiddleFingerJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getJointDefinition(RightWristPitchName), RightPinkyJointNames, robotDefinition);
      assertKinematicsContinuity(robotDefinition.getJointDefinition(RightWristPitchName), RightThumbJointNames, robotDefinition);

      assertPhysicalProperties(robotDefinition, valkyrieProperties(), subtract(AllJointNames, HokuyoJointName), AllLinkNames);
      assertSensorsProperties(robotDefinition, valkyrieSensorProperties(), AllJointNames);
   }

   public static void assertPhysicalProperties(RobotDefinition robotDefinition,
                                               Map> robotProperties,
                                               String[] allJointNames,
                                               String[] allLinkNames)
   {
      for (String jointName : allJointNames)
      {
         Map jointProperties = robotProperties.get(jointName);
         JointDefinition jointDefinition = robotDefinition.getJointDefinition(jointName);
         String messagePrefix = "Joint: " + jointName;
         Orientation3DBasics rotation = jointDefinition.getTransformToParent().getRotation();
         Vector3D translation = jointDefinition.getTransformToParent().getTranslation();
         assertTrue(rotation.isZeroOrientation(EPSILON), "Expected zero rotation, was: " + rotation);
         EuclidCoreTestTools.assertEquals(messagePrefix, (Tuple3DReadOnly) jointProperties.get("offsetFromParentJoint"), translation, EPSILON);
         if (jointDefinition instanceof OneDoFJointDefinition)
         {
            OneDoFJointDefinition oneDoFJointDefinition = (OneDoFJointDefinition) jointDefinition;
            assertEquals((double) jointProperties.get("positionLowerLimit"), oneDoFJointDefinition.getPositionLowerLimit(), messagePrefix);
            assertEquals((double) jointProperties.get("positionUpperLimit"), oneDoFJointDefinition.getPositionUpperLimit(), messagePrefix);
            assertEquals((double) jointProperties.get("velocityLowerLimit"), oneDoFJointDefinition.getVelocityLowerLimit(), messagePrefix);
            assertEquals((double) jointProperties.get("velocityUpperLimit"), oneDoFJointDefinition.getVelocityUpperLimit(), messagePrefix);
            assertEquals((double) jointProperties.get("effortLowerLimit"), oneDoFJointDefinition.getEffortLowerLimit(), messagePrefix);
            assertEquals((double) jointProperties.get("effortUpperLimit"), oneDoFJointDefinition.getEffortUpperLimit(), messagePrefix);
            EuclidCoreTestTools.assertEquals(messagePrefix, (Tuple3DReadOnly) jointProperties.get("axis"), oneDoFJointDefinition.getAxis(), EPSILON);
            assertEquals((double) jointProperties.get("damping"), oneDoFJointDefinition.getDamping(), messagePrefix);
            assertEquals((double) jointProperties.get("stiction"), oneDoFJointDefinition.getStiction(), messagePrefix);
         }
      }

      for (String linkName : allLinkNames)
      {
         String messagePrefix = "Link: " + linkName;
         Map linkProperties = robotProperties.get(linkName);
         RigidBodyDefinition rigidBodyDefinition = robotDefinition.getRigidBodyDefinition(linkName);
         assertEquals((double) linkProperties.get("mass"), rigidBodyDefinition.getMass(), EPSILON, messagePrefix);
         EuclidCoreTestTools.assertEquals(messagePrefix,
                                          (Tuple3DReadOnly) linkProperties.get("centerOfMass"),
                                          rigidBodyDefinition.getCenterOfMassOffset(),
                                          EPSILON);
         EuclidCoreTestTools.assertMatrix3DEquals(messagePrefix,
                                                  (Matrix3DReadOnly) linkProperties.get("inertia"),
                                                  rigidBodyDefinition.getMomentOfInertia(),
                                                  EPSILON);
      }
   }

   public static void assertSensorsProperties(RobotDefinition robotDefinition, Map> robotSensorProperties, String[] allJointNames)
   {
      int actualNumberOfCameras = 0;
      int actualNumberOfIMUs = 0;
      int actualNumberOfLidars = 0;

      for (String jointName : allJointNames)
      {
         JointDefinition jointDefinition = robotDefinition.getJointDefinition(jointName);

         actualNumberOfCameras += jointDefinition.getSensorDefinitions(CameraSensorDefinition.class).size();
         actualNumberOfIMUs += jointDefinition.getSensorDefinitions(IMUSensorDefinition.class).size();
         actualNumberOfLidars += jointDefinition.getSensorDefinitions(LidarSensorDefinition.class).size();

         for (SensorDefinition sensorDefinition : jointDefinition.getSensorDefinitions())
         {
            Map sensorProperties = robotSensorProperties.get(sensorDefinition.getName());
            assertSensorProperties(sensorDefinition, sensorProperties);
         }
      }

      int expectedNumberOfCameras = (int) robotSensorProperties.values()
                                                               .stream()
                                                               .flatMap(sensorProperties -> sensorProperties.entrySet().stream())
                                                               .filter(entry -> entry.getKey().equals("imageWidth"))
                                                               .count();
      int expectedNumberOfIMUs = (int) robotSensorProperties.values()
                                                            .stream()
                                                            .flatMap(sensorProperties -> sensorProperties.entrySet().stream())
                                                            .filter(entry -> entry.getKey().equals("accelerationNoiseMean"))
                                                            .count();
      int expectedNumberOfLidars = (int) robotSensorProperties.values()
                                                              .stream()
                                                              .flatMap(sensorProperties -> sensorProperties.entrySet().stream())
                                                              .filter(entry -> entry.getKey().equals("sweepYawMin"))
                                                              .count();
      assertEquals(expectedNumberOfCameras, actualNumberOfCameras);
      assertEquals(expectedNumberOfIMUs, actualNumberOfIMUs);
      assertEquals(expectedNumberOfLidars, actualNumberOfLidars);
   }

   public static void assertSensorProperties(SensorDefinition sensorDefinition, Map sensorProperties)
   {
      if (sensorProperties == null)
         return;

      EuclidCoreTestTools.assertEquals("Sensor: " + sensorDefinition.getName(),
                                       (RigidBodyTransform) sensorProperties.get("transformToJoint"),
                                       new RigidBodyTransform(sensorDefinition.getTransformToJoint()),
                                       EPSILON);

      if (sensorDefinition instanceof CameraSensorDefinition)
      {
         CameraSensorDefinition cameraSensorDefinition = (CameraSensorDefinition) sensorDefinition;
         assertEquals((double) sensorProperties.get("fieldOfView"), cameraSensorDefinition.getFieldOfView());
         assertEquals((double) sensorProperties.get("clipNear"), cameraSensorDefinition.getClipNear());
         assertEquals((double) sensorProperties.get("clipFar"), cameraSensorDefinition.getClipFar());
         assertEquals((int) sensorProperties.get("imageWidth"), cameraSensorDefinition.getImageWidth());
         assertEquals((int) sensorProperties.get("imageHeight"), cameraSensorDefinition.getImageHeight());
      }
      else if (sensorDefinition instanceof IMUSensorDefinition)
      {
         IMUSensorDefinition imuSensorDefinition = (IMUSensorDefinition) sensorDefinition;
         assertEquals((double) sensorProperties.get("accelerationNoiseMean"), imuSensorDefinition.getAccelerationNoiseMean());
         assertEquals((double) sensorProperties.get("accelerationNoiseStandardDeviation"), imuSensorDefinition.getAccelerationNoiseStandardDeviation());
         assertEquals((double) sensorProperties.get("accelerationBiasMean"), imuSensorDefinition.getAccelerationBiasMean());
         assertEquals((double) sensorProperties.get("accelerationBiasStandardDeviation"), imuSensorDefinition.getAccelerationBiasStandardDeviation());
         assertEquals((double) sensorProperties.get("angularVelocityNoiseMean"), imuSensorDefinition.getAngularVelocityNoiseMean());
         assertEquals((double) sensorProperties.get("angularVelocityNoiseStandardDeviation"), imuSensorDefinition.getAngularVelocityNoiseStandardDeviation());
         assertEquals((double) sensorProperties.get("angularVelocityBiasMean"), imuSensorDefinition.getAngularVelocityBiasMean());
         assertEquals((double) sensorProperties.get("angularVelocityBiasStandardDeviation"), imuSensorDefinition.getAngularVelocityBiasStandardDeviation());
      }
      else if (sensorDefinition instanceof LidarSensorDefinition)
      {
         LidarSensorDefinition lidarSensorDefinition = (LidarSensorDefinition) sensorDefinition;
         assertEquals((double) sensorProperties.get("sweepYawMin"), lidarSensorDefinition.getSweepYawMin());
         assertEquals((double) sensorProperties.get("sweepYawMax"), lidarSensorDefinition.getSweepYawMax());
         assertEquals((double) sensorProperties.get("heightPitchMin"), lidarSensorDefinition.getHeightPitchMin());
         assertEquals((double) sensorProperties.get("heightPitchMax"), lidarSensorDefinition.getHeightPitchMax());
         assertEquals((double) sensorProperties.get("minRange"), lidarSensorDefinition.getMinRange());
         assertEquals((double) sensorProperties.get("maxRange"), lidarSensorDefinition.getMaxRange());
         assertEquals((int) sensorProperties.get("pointsPerSweep"), lidarSensorDefinition.getPointsPerSweep());
         assertEquals((int) sensorProperties.get("scanHeight"), lidarSensorDefinition.getScanHeight());
      }
   }

   public static void assertKinematicsContinuity(JointDefinition expectedParentJoint, String[] jointNames, RobotDefinition robotDefinition)
   {
      for (String jointName : jointNames)
      {
         JointDefinition joint = robotDefinition.getJointDefinition(jointName);
         assertNotNull(joint.getParentJoint(), jointName);
         assertTrue(expectedParentJoint == joint.getParentJoint());
         assertTrue(expectedParentJoint.getSuccessor().getChildrenJoints().contains(joint));
         expectedParentJoint = joint;
      }
   }

   @SuppressWarnings({"unchecked"})
   public static  T[] concatenate(T[]... arrays)
   {
      int length = Stream.of(arrays).mapToInt(array -> array.length).sum();
      T[] ret = (T[]) Array.newInstance(arrays[0].getClass().getComponentType(), length);

      int currentIndex = 0;

      for (T[] array : arrays)
      {
         for (T element : array)
         {
            ret[currentIndex++] = element;
         }
      }
      return ret;
   }

   @SuppressWarnings("unchecked")
   @SafeVarargs
   public static  T[] subtract(T[] source, T... elementsToSubtract)
   {
      HashSet sourceSet = new LinkedHashSet<>(Arrays.asList(source));
      for (T elementToSubtract : elementsToSubtract)
      {
         sourceSet.remove(elementToSubtract);
      }
      return sourceSet.toArray((T[]) Array.newInstance(source.getClass().getComponentType(), sourceSet.size()));
   }

   private static final double Infinity = Double.POSITIVE_INFINITY;

   private static Map> valkyrieProperties()
   {
      // Generated from working parser.
      Map> properties = new HashMap<>();
      properties.put("pelvis", new HashMap<>());
      properties.put("pelvis", new HashMap<>());
      properties.get("pelvis").put("mass", 8.22);
      properties.get("pelvis").put("centerOfMass", new Vector3D(-0.00532, -0.003512, -0.0036));
      properties.get("pelvis")
                .put("inertia", new Matrix3D(0.118664, -1.43482E-4, 0.00327129, -1.43482E-4, 0.0979634, 0.00215955, 0.00327129, 0.00215955, 0.0838546));
      properties.get("pelvis").put("offsetFromParentJoint", new Vector3D(0.0, 0.0, 0.0));
      properties.put("leftHipYaw", new HashMap<>());
      properties.put("leftHipYawLink", new HashMap<>());
      properties.get("leftHipYawLink").put("mass", 2.39);
      properties.get("leftHipYawLink").put("centerOfMass", new Vector3D(0.02176, -0.00131, 0.03867));
      properties.get("leftHipYawLink").put("inertia", new Matrix3D(0.017261, 0.0, 0.0, 0.0, 0.0148662, 0.0, 0.0, 0.0, 0.0112382));
      properties.get("leftHipYaw").put("offsetFromParentJoint", new Vector3D(0.0, 0.1016, -0.1853));
      properties.get("leftHipYaw").put("positionLowerLimit", -0.4141);
      properties.get("leftHipYaw").put("positionUpperLimit", 1.1);
      properties.get("leftHipYaw").put("velocityLowerLimit", -5.89);
      properties.get("leftHipYaw").put("velocityUpperLimit", 5.89);
      properties.get("leftHipYaw").put("effortLowerLimit", -190.0);
      properties.get("leftHipYaw").put("effortUpperLimit", 190.0);
      properties.get("leftHipYaw").put("kpPositionLimit", 100.0);
      properties.get("leftHipYaw").put("kdPositionLimit", 20.0);
      properties.get("leftHipYaw").put("kpVelocityLimit", 500.0);
      properties.get("leftHipYaw").put("axis", new Vector3D(0.0, 0.0, 1.0));
      properties.get("leftHipYaw").put("damping", 0.1);
      properties.get("leftHipYaw").put("stiction", 0.0);
      properties.put("leftHipRoll", new HashMap<>());
      properties.put("leftHipRollLink", new HashMap<>());
      properties.get("leftHipRollLink").put("mass", 3.665);
      properties.get("leftHipRollLink").put("centerOfMass", new Vector3D(0.012959, 0.00755, -0.01595));
      properties.get("leftHipRollLink")
                .put("inertia", new Matrix3D(0.00597896, -2.34823E-4, 5.53962E-4, -2.34823E-4, 0.00937265, 7.78956E-4, 5.53962E-4, 7.78956E-4, 0.00819312));
      properties.get("leftHipRoll").put("offsetFromParentJoint", new Vector3D(0.0, 0.0, 0.0));
      properties.get("leftHipRoll").put("positionLowerLimit", -0.467);
      properties.get("leftHipRoll").put("positionUpperLimit", 0.5515);
      properties.get("leftHipRoll").put("velocityLowerLimit", -7.0);
      properties.get("leftHipRoll").put("velocityUpperLimit", 7.0);
      properties.get("leftHipRoll").put("effortLowerLimit", -350.0);
      properties.get("leftHipRoll").put("effortUpperLimit", 350.0);
      properties.get("leftHipRoll").put("kpPositionLimit", 100.0);
      properties.get("leftHipRoll").put("kdPositionLimit", 20.0);
      properties.get("leftHipRoll").put("kpVelocityLimit", 500.0);
      properties.get("leftHipRoll").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("leftHipRoll").put("damping", 0.1);
      properties.get("leftHipRoll").put("stiction", 0.0);
      properties.put("leftHipPitch", new HashMap<>());
      properties.put("leftHipPitchLink", new HashMap<>());
      properties.get("leftHipPitchLink").put("mass", 10.2);
      properties.get("leftHipPitchLink").put("centerOfMass", new Vector3D(0.016691, 0.091397, -0.207875));
      properties.get("leftHipPitchLink")
                .put("inertia", new Matrix3D(0.240834, 3.5915E-5, 0.00369938, 3.5915E-5, 0.256897, -0.001333, 0.00369938, -0.001333, 0.0232764));
      properties.get("leftHipPitch").put("offsetFromParentJoint", new Vector3D(0.0, 0.0, -0.06090000000000001));
      properties.get("leftHipPitch").put("positionLowerLimit", -2.42);
      properties.get("leftHipPitch").put("positionUpperLimit", 1.619);
      properties.get("leftHipPitch").put("velocityLowerLimit", -6.11);
      properties.get("leftHipPitch").put("velocityUpperLimit", 6.11);
      properties.get("leftHipPitch").put("effortLowerLimit", -350.0);
      properties.get("leftHipPitch").put("effortUpperLimit", 350.0);
      properties.get("leftHipPitch").put("kpPositionLimit", 100.0);
      properties.get("leftHipPitch").put("kdPositionLimit", 20.0);
      properties.get("leftHipPitch").put("kpVelocityLimit", 500.0);
      properties.get("leftHipPitch").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("leftHipPitch").put("damping", 0.1);
      properties.get("leftHipPitch").put("stiction", 0.0);
      properties.put("leftKneePitch", new HashMap<>());
      properties.put("leftKneePitchLink", new HashMap<>());
      properties.get("leftKneePitchLink").put("mass", 6.2);
      properties.get("leftKneePitchLink").put("centerOfMass", new Vector3D(-0.022183, 0.001703, -0.189418));
      properties.get("leftKneePitchLink")
                .put("inertia", new Matrix3D(0.0869357, 9.929E-5, 5.73207E-4, 9.929E-5, 0.0915841, 3.13745E-4, 5.73207E-4, 3.13745E-4, 0.0140173));
      properties.get("leftKneePitch").put("offsetFromParentJoint", new Vector3D(1.12225E-4, 0.036105, -0.430959));
      properties.get("leftKneePitch").put("positionLowerLimit", -0.083);
      properties.get("leftKneePitch").put("positionUpperLimit", 2.057);
      properties.get("leftKneePitch").put("velocityLowerLimit", -6.11);
      properties.get("leftKneePitch").put("velocityUpperLimit", 6.11);
      properties.get("leftKneePitch").put("effortLowerLimit", -350.0);
      properties.get("leftKneePitch").put("effortUpperLimit", 350.0);
      properties.get("leftKneePitch").put("kpPositionLimit", 100.0);
      properties.get("leftKneePitch").put("kdPositionLimit", 20.0);
      properties.get("leftKneePitch").put("kpVelocityLimit", 500.0);
      properties.get("leftKneePitch").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("leftKneePitch").put("damping", 0.1);
      properties.get("leftKneePitch").put("stiction", 0.0);
      properties.put("leftAnklePitch", new HashMap<>());
      properties.put("leftAnklePitchLink", new HashMap<>());
      properties.get("leftAnklePitchLink").put("mass", 0.03);
      properties.get("leftAnklePitchLink").put("centerOfMass", new Vector3D(-0.0, -0.0, -0.0));
      properties.get("leftAnklePitchLink").put("inertia", new Matrix3D(4.377E-6, 0.0, 0.0, 0.0, 4.322E-6, 0.0, 0.0, 0.0, 7.015E-6));
      properties.get("leftAnklePitch").put("offsetFromParentJoint", new Vector3D(-0.010238125, 0.0, -0.40627099999999994));
      properties.get("leftAnklePitch").put("positionLowerLimit", -0.8644);
      properties.get("leftAnklePitch").put("positionUpperLimit", 0.875);
      properties.get("leftAnklePitch").put("velocityLowerLimit", -11.0);
      properties.get("leftAnklePitch").put("velocityUpperLimit", 11.0);
      properties.get("leftAnklePitch").put("effortLowerLimit", -205.0);
      properties.get("leftAnklePitch").put("effortUpperLimit", 205.0);
      properties.get("leftAnklePitch").put("kpPositionLimit", 100.0);
      properties.get("leftAnklePitch").put("kdPositionLimit", 20.0);
      properties.get("leftAnklePitch").put("kpVelocityLimit", 500.0);
      properties.get("leftAnklePitch").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("leftAnklePitch").put("damping", 0.1);
      properties.get("leftAnklePitch").put("stiction", 0.0);
      properties.put("leftAnkleRoll", new HashMap<>());
      properties.put("leftFoot", new HashMap<>());
      properties.get("leftFoot").put("mass", 2.37);
      properties.get("leftFoot").put("centerOfMass", new Vector3D(0.0369087, 0.00494324, -0.0489279));
      properties.get("leftFoot")
                .put("inertia", new Matrix3D(0.00641532, 2.0788E-4, 0.00128536, 2.0788E-4, 0.0179943, -2.02908E-4, 0.00128536, -2.02908E-4, 0.0209358));
      properties.get("leftAnkleRoll").put("offsetFromParentJoint", new Vector3D(0.0101259, 0.0, 0.0));
      properties.get("leftAnkleRoll").put("positionLowerLimit", -0.349);
      properties.get("leftAnkleRoll").put("positionUpperLimit", 0.348);
      properties.get("leftAnkleRoll").put("velocityLowerLimit", -11.0);
      properties.get("leftAnkleRoll").put("velocityUpperLimit", 11.0);
      properties.get("leftAnkleRoll").put("effortLowerLimit", -205.0);
      properties.get("leftAnkleRoll").put("effortUpperLimit", 205.0);
      properties.get("leftAnkleRoll").put("kpPositionLimit", 100.0);
      properties.get("leftAnkleRoll").put("kdPositionLimit", 20.0);
      properties.get("leftAnkleRoll").put("kpVelocityLimit", 500.0);
      properties.get("leftAnkleRoll").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("leftAnkleRoll").put("damping", 0.3);
      properties.get("leftAnkleRoll").put("stiction", 0.0);
      properties.put("rightHipYaw", new HashMap<>());
      properties.put("rightHipYawLink", new HashMap<>());
      properties.get("rightHipYawLink").put("mass", 2.39);
      properties.get("rightHipYawLink").put("centerOfMass", new Vector3D(0.02176, 0.00131, 0.03867));
      properties.get("rightHipYawLink").put("inertia", new Matrix3D(0.017261, 0.0, 0.0, 0.0, 0.0148662, 0.0, 0.0, 0.0, 0.0112382));
      properties.get("rightHipYaw").put("offsetFromParentJoint", new Vector3D(0.0, -0.1016, -0.1853));
      properties.get("rightHipYaw").put("positionLowerLimit", -1.1);
      properties.get("rightHipYaw").put("positionUpperLimit", 0.4141);
      properties.get("rightHipYaw").put("velocityLowerLimit", -5.89);
      properties.get("rightHipYaw").put("velocityUpperLimit", 5.89);
      properties.get("rightHipYaw").put("effortLowerLimit", -190.0);
      properties.get("rightHipYaw").put("effortUpperLimit", 190.0);
      properties.get("rightHipYaw").put("kpPositionLimit", 100.0);
      properties.get("rightHipYaw").put("kdPositionLimit", 20.0);
      properties.get("rightHipYaw").put("kpVelocityLimit", 500.0);
      properties.get("rightHipYaw").put("axis", new Vector3D(0.0, 0.0, 1.0));
      properties.get("rightHipYaw").put("damping", 0.1);
      properties.get("rightHipYaw").put("stiction", 0.0);
      properties.put("rightHipRoll", new HashMap<>());
      properties.put("rightHipRollLink", new HashMap<>());
      properties.get("rightHipRollLink").put("mass", 3.665);
      properties.get("rightHipRollLink").put("centerOfMass", new Vector3D(0.012959, -0.00755, -0.01595));
      properties.get("rightHipRollLink")
                .put("inertia", new Matrix3D(0.00597896, 2.34823E-4, 5.53962E-4, 2.34823E-4, 0.00937265, -7.78956E-4, 5.53962E-4, -7.78956E-4, 0.00819312));
      properties.get("rightHipRoll").put("offsetFromParentJoint", new Vector3D(0.0, 0.0, 0.0));
      properties.get("rightHipRoll").put("positionLowerLimit", -0.5515);
      properties.get("rightHipRoll").put("positionUpperLimit", 0.467);
      properties.get("rightHipRoll").put("velocityLowerLimit", -7.0);
      properties.get("rightHipRoll").put("velocityUpperLimit", 7.0);
      properties.get("rightHipRoll").put("effortLowerLimit", -350.0);
      properties.get("rightHipRoll").put("effortUpperLimit", 350.0);
      properties.get("rightHipRoll").put("kpPositionLimit", 100.0);
      properties.get("rightHipRoll").put("kdPositionLimit", 20.0);
      properties.get("rightHipRoll").put("kpVelocityLimit", 500.0);
      properties.get("rightHipRoll").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("rightHipRoll").put("damping", 0.1);
      properties.get("rightHipRoll").put("stiction", 0.0);
      properties.put("rightHipPitch", new HashMap<>());
      properties.put("rightHipPitchLink", new HashMap<>());
      properties.get("rightHipPitchLink").put("mass", 10.2);
      properties.get("rightHipPitchLink").put("centerOfMass", new Vector3D(0.016691, -0.091397, -0.207875));
      properties.get("rightHipPitchLink")
                .put("inertia", new Matrix3D(0.240834, -3.5915E-5, 0.00369938, -3.5915E-5, 0.256897, 0.001333, 0.00369938, 0.001333, 0.0232764));
      properties.get("rightHipPitch").put("offsetFromParentJoint", new Vector3D(0.0, 0.0, -0.06090000000000001));
      properties.get("rightHipPitch").put("positionLowerLimit", -2.42);
      properties.get("rightHipPitch").put("positionUpperLimit", 1.619);
      properties.get("rightHipPitch").put("velocityLowerLimit", -6.11);
      properties.get("rightHipPitch").put("velocityUpperLimit", 6.11);
      properties.get("rightHipPitch").put("effortLowerLimit", -350.0);
      properties.get("rightHipPitch").put("effortUpperLimit", 350.0);
      properties.get("rightHipPitch").put("kpPositionLimit", 100.0);
      properties.get("rightHipPitch").put("kdPositionLimit", 20.0);
      properties.get("rightHipPitch").put("kpVelocityLimit", 500.0);
      properties.get("rightHipPitch").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("rightHipPitch").put("damping", 0.1);
      properties.get("rightHipPitch").put("stiction", 0.0);
      properties.put("rightKneePitch", new HashMap<>());
      properties.put("rightKneePitchLink", new HashMap<>());
      properties.get("rightKneePitchLink").put("mass", 6.2);
      properties.get("rightKneePitchLink").put("centerOfMass", new Vector3D(-0.022183, 0.001703, -0.189418));
      properties.get("rightKneePitchLink")
                .put("inertia", new Matrix3D(0.0869357, 9.929E-5, 5.73207E-4, 9.929E-5, 0.0915841, 3.13745E-4, 5.73207E-4, 3.13745E-4, 0.0140173));
      properties.get("rightKneePitch").put("offsetFromParentJoint", new Vector3D(1.12225E-4, -0.036105, -0.430959));
      properties.get("rightKneePitch").put("positionLowerLimit", -0.083);
      properties.get("rightKneePitch").put("positionUpperLimit", 2.057);
      properties.get("rightKneePitch").put("velocityLowerLimit", -6.11);
      properties.get("rightKneePitch").put("velocityUpperLimit", 6.11);
      properties.get("rightKneePitch").put("effortLowerLimit", -350.0);
      properties.get("rightKneePitch").put("effortUpperLimit", 350.0);
      properties.get("rightKneePitch").put("kpPositionLimit", 100.0);
      properties.get("rightKneePitch").put("kdPositionLimit", 20.0);
      properties.get("rightKneePitch").put("kpVelocityLimit", 500.0);
      properties.get("rightKneePitch").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("rightKneePitch").put("damping", 0.1);
      properties.get("rightKneePitch").put("stiction", 0.0);
      properties.put("rightAnklePitch", new HashMap<>());
      properties.put("rightAnklePitchLink", new HashMap<>());
      properties.get("rightAnklePitchLink").put("mass", 0.03);
      properties.get("rightAnklePitchLink").put("centerOfMass", new Vector3D(-0.0, -0.0, -0.0));
      properties.get("rightAnklePitchLink").put("inertia", new Matrix3D(4.377E-6, 0.0, 0.0, 0.0, 4.322E-6, 0.0, 0.0, 0.0, 7.015E-6));
      properties.get("rightAnklePitch").put("offsetFromParentJoint", new Vector3D(-0.010238125, 0.0, -0.40627099999999994));
      properties.get("rightAnklePitch").put("positionLowerLimit", -0.8644);
      properties.get("rightAnklePitch").put("positionUpperLimit", 0.875);
      properties.get("rightAnklePitch").put("velocityLowerLimit", -11.0);
      properties.get("rightAnklePitch").put("velocityUpperLimit", 11.0);
      properties.get("rightAnklePitch").put("effortLowerLimit", -205.0);
      properties.get("rightAnklePitch").put("effortUpperLimit", 205.0);
      properties.get("rightAnklePitch").put("kpPositionLimit", 100.0);
      properties.get("rightAnklePitch").put("kdPositionLimit", 20.0);
      properties.get("rightAnklePitch").put("kpVelocityLimit", 500.0);
      properties.get("rightAnklePitch").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("rightAnklePitch").put("damping", 0.1);
      properties.get("rightAnklePitch").put("stiction", 0.0);
      properties.put("rightAnkleRoll", new HashMap<>());
      properties.put("rightFoot", new HashMap<>());
      properties.get("rightFoot").put("mass", 2.37);
      properties.get("rightFoot").put("centerOfMass", new Vector3D(0.0369087, 0.00494324, -0.0489279));
      properties.get("rightFoot")
                .put("inertia", new Matrix3D(0.00641532, 2.0788E-4, 0.00128536, 2.0788E-4, 0.0179943, -2.02908E-4, 0.00128536, -2.02908E-4, 0.0209358));
      properties.get("rightAnkleRoll").put("offsetFromParentJoint", new Vector3D(0.0101259, 0.0, 0.0));
      properties.get("rightAnkleRoll").put("positionLowerLimit", -0.349);
      properties.get("rightAnkleRoll").put("positionUpperLimit", 0.348);
      properties.get("rightAnkleRoll").put("velocityLowerLimit", -11.0);
      properties.get("rightAnkleRoll").put("velocityUpperLimit", 11.0);
      properties.get("rightAnkleRoll").put("effortLowerLimit", -205.0);
      properties.get("rightAnkleRoll").put("effortUpperLimit", 205.0);
      properties.get("rightAnkleRoll").put("kpPositionLimit", 100.0);
      properties.get("rightAnkleRoll").put("kdPositionLimit", 20.0);
      properties.get("rightAnkleRoll").put("kpVelocityLimit", 500.0);
      properties.get("rightAnkleRoll").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("rightAnkleRoll").put("damping", 0.3);
      properties.get("rightAnkleRoll").put("stiction", 0.0);
      properties.put("torsoYaw", new HashMap<>());
      properties.put("torsoYawLink", new HashMap<>());
      properties.get("torsoYawLink").put("mass", 0.5);
      properties.get("torsoYawLink").put("centerOfMass", new Vector3D(0.0, 0.0, -0.01));
      properties.get("torsoYawLink")
                .put("inertia", new Matrix3D(6.08427E-4, -1.172E-6, 1.647E-6, -1.172E-6, 6.20328E-4, -2.33E-7, 1.647E-6, -2.33E-7, 0.00107811));
      properties.get("torsoYaw").put("offsetFromParentJoint", new Vector3D(-0.0, -0.0, -0.0));
      properties.get("torsoYaw").put("positionLowerLimit", -1.329);
      properties.get("torsoYaw").put("positionUpperLimit", 1.181);
      properties.get("torsoYaw").put("velocityLowerLimit", -5.89);
      properties.get("torsoYaw").put("velocityUpperLimit", 5.89);
      properties.get("torsoYaw").put("effortLowerLimit", -190.0);
      properties.get("torsoYaw").put("effortUpperLimit", 190.0);
      properties.get("torsoYaw").put("kpPositionLimit", 100.0);
      properties.get("torsoYaw").put("kdPositionLimit", 20.0);
      properties.get("torsoYaw").put("kpVelocityLimit", 500.0);
      properties.get("torsoYaw").put("axis", new Vector3D(0.0, 0.0, 1.0));
      properties.get("torsoYaw").put("damping", 0.1);
      properties.get("torsoYaw").put("stiction", 0.0);
      properties.put("torsoPitch", new HashMap<>());
      properties.put("torsoPitchLink", new HashMap<>());
      properties.get("torsoPitchLink").put("mass", 0.1);
      properties.get("torsoPitchLink").put("centerOfMass", new Vector3D(0.0, 0.0, 0.005));
      properties.get("torsoPitchLink").put("inertia", new Matrix3D(3.032E-5, 0.0, -1.145E-6, 0.0, 2.1274E-5, 0.0, -1.145E-6, 0.0, 2.8285E-5));
      properties.get("torsoPitch").put("offsetFromParentJoint", new Vector3D(0.04191, 0.0, 0.0));
      properties.get("torsoPitch").put("positionLowerLimit", -0.13);
      properties.get("torsoPitch").put("positionUpperLimit", 0.666);
      properties.get("torsoPitch").put("velocityLowerLimit", -9.0);
      properties.get("torsoPitch").put("velocityUpperLimit", 9.0);
      properties.get("torsoPitch").put("effortLowerLimit", -150.0);
      properties.get("torsoPitch").put("effortUpperLimit", 150.0);
      properties.get("torsoPitch").put("kpPositionLimit", 100.0);
      properties.get("torsoPitch").put("kdPositionLimit", 20.0);
      properties.get("torsoPitch").put("kpVelocityLimit", 500.0);
      properties.get("torsoPitch").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("torsoPitch").put("damping", 0.1);
      properties.get("torsoPitch").put("stiction", 0.0);
      properties.put("torsoRoll", new HashMap<>());
      properties.put("torso", new HashMap<>());
      properties.get("torso").put("mass", 39.47);
      properties.get("torso").put("centerOfMass", new Vector3D(-0.095548, -0.003337, 0.243098));
      properties.get("torso").put("inertia", new Matrix3D(0.873269, 9.95625E-5, 0.0613452, 9.95625E-5, 1.01085, 0.00181849, 0.0613452, 0.00181849, 0.778398));
      properties.get("torsoRoll").put("offsetFromParentJoint", new Vector3D(0.0, 0.0, 0.0203));
      properties.get("torsoRoll").put("positionLowerLimit", -0.23);
      properties.get("torsoRoll").put("positionUpperLimit", 0.255);
      properties.get("torsoRoll").put("velocityLowerLimit", -9.0);
      properties.get("torsoRoll").put("velocityUpperLimit", 9.0);
      properties.get("torsoRoll").put("effortLowerLimit", -150.0);
      properties.get("torsoRoll").put("effortUpperLimit", 150.0);
      properties.get("torsoRoll").put("kpPositionLimit", 100.0);
      properties.get("torsoRoll").put("kdPositionLimit", 20.0);
      properties.get("torsoRoll").put("kpVelocityLimit", 500.0);
      properties.get("torsoRoll").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("torsoRoll").put("damping", 0.1);
      properties.get("torsoRoll").put("stiction", 0.0);
      properties.put("leftShoulderPitch", new HashMap<>());
      properties.put("leftShoulderPitchLink", new HashMap<>());
      properties.get("leftShoulderPitchLink").put("mass", 2.65);
      properties.get("leftShoulderPitchLink").put("centerOfMass", new Vector3D(-0.012, 0.251, 0.0));
      properties.get("leftShoulderPitchLink").put("inertia", new Matrix3D(0.0137182, 0.0, 0.0, 0.0, 0.0105028, 0.0, 0.0, 0.0, 0.0148064));
      properties.get("leftShoulderPitch").put("offsetFromParentJoint", new Vector3D(-0.0316, 0.0, 0.2984));
      properties.get("leftShoulderPitch").put("positionLowerLimit", -2.85);
      properties.get("leftShoulderPitch").put("positionUpperLimit", 2.0);
      properties.get("leftShoulderPitch").put("velocityLowerLimit", -3.0);
      properties.get("leftShoulderPitch").put("velocityUpperLimit", 3.0);
      properties.get("leftShoulderPitch").put("effortLowerLimit", -190.0);
      properties.get("leftShoulderPitch").put("effortUpperLimit", 190.0);
      properties.get("leftShoulderPitch").put("kpPositionLimit", 100.0);
      properties.get("leftShoulderPitch").put("kdPositionLimit", 20.0);
      properties.get("leftShoulderPitch").put("kpVelocityLimit", 500.0);
      properties.get("leftShoulderPitch").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("leftShoulderPitch").put("damping", 0.1);
      properties.get("leftShoulderPitch").put("stiction", 0.0);
      properties.put("leftShoulderRoll", new HashMap<>());
      properties.put("leftShoulderRollLink", new HashMap<>());
      properties.get("leftShoulderRollLink").put("mass", 3.97);
      properties.get("leftShoulderRollLink").put("centerOfMass", new Vector3D(-0.008513, 0.02068, -0.001088));
      properties.get("leftShoulderRollLink")
                .put("inertia", new Matrix3D(0.0145988, -6.6764E-4, -3.629E-5, -6.6764E-4, 0.00645214, -8.283E-5, -3.629E-5, -8.283E-5, 0.0168483));
      properties.get("leftShoulderRoll").put("offsetFromParentJoint", new Vector3D(0.0, 0.2499, 0.0));
      properties.get("leftShoulderRoll").put("positionLowerLimit", -1.519);
      properties.get("leftShoulderRoll").put("positionUpperLimit", 1.266);
      properties.get("leftShoulderRoll").put("velocityLowerLimit", -3.5);
      properties.get("leftShoulderRoll").put("velocityUpperLimit", 3.5);
      properties.get("leftShoulderRoll").put("effortLowerLimit", -350.0);
      properties.get("leftShoulderRoll").put("effortUpperLimit", 350.0);
      properties.get("leftShoulderRoll").put("kpPositionLimit", 100.0);
      properties.get("leftShoulderRoll").put("kdPositionLimit", 20.0);
      properties.get("leftShoulderRoll").put("kpVelocityLimit", 500.0);
      properties.get("leftShoulderRoll").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("leftShoulderRoll").put("damping", 0.1);
      properties.get("leftShoulderRoll").put("stiction", 0.0);
      properties.put("leftShoulderYaw", new HashMap<>());
      properties.put("leftShoulderYawLink", new HashMap<>());
      properties.get("leftShoulderYawLink").put("mass", 3.085);
      properties.get("leftShoulderYawLink").put("centerOfMass", new Vector3D(-0.004304, 0.209832, 0.007295));
      properties.get("leftShoulderYawLink")
                .put("inertia", new Matrix3D(0.0393552, -0.00782708, -7.53947E-4, -0.00782708, 0.00490577, 0.00272387, -7.53947E-4, 0.00272387, 0.0418795));
      properties.get("leftShoulderYaw").put("offsetFromParentJoint", new Vector3D(0.0, 0.0, 0.0));
      properties.get("leftShoulderYaw").put("positionLowerLimit", -3.1);
      properties.get("leftShoulderYaw").put("positionUpperLimit", 2.18);
      properties.get("leftShoulderYaw").put("velocityLowerLimit", -1.5);
      properties.get("leftShoulderYaw").put("velocityUpperLimit", 1.5);
      properties.get("leftShoulderYaw").put("effortLowerLimit", -65.0);
      properties.get("leftShoulderYaw").put("effortUpperLimit", 65.0);
      properties.get("leftShoulderYaw").put("kpPositionLimit", 100.0);
      properties.get("leftShoulderYaw").put("kdPositionLimit", 20.0);
      properties.get("leftShoulderYaw").put("kpVelocityLimit", 500.0);
      properties.get("leftShoulderYaw").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("leftShoulderYaw").put("damping", 0.1);
      properties.get("leftShoulderYaw").put("stiction", 0.0);
      properties.put("leftElbowPitch", new HashMap<>());
      properties.put("leftElbowPitchLink", new HashMap<>());
      properties.get("leftElbowPitchLink").put("mass", 1.83);
      properties.get("leftElbowPitchLink").put("centerOfMass", new Vector3D(-0.020344, 0.014722, 0.0223));
      properties.get("leftElbowPitchLink")
                .put("inertia", new Matrix3D(0.00331452, 5.35099E-4, 7.28077E-4, 5.35099E-4, 0.00350567, -4.23865E-4, 7.28077E-4, -4.23865E-4, 0.00301128));
      properties.get("leftElbowPitch").put("offsetFromParentJoint", new Vector3D(0.0254, 0.32999999999999996, 0.0));
      properties.get("leftElbowPitch").put("positionLowerLimit", -2.174);
      properties.get("leftElbowPitch").put("positionUpperLimit", 0.12);
      properties.get("leftElbowPitch").put("velocityLowerLimit", -3.5);
      properties.get("leftElbowPitch").put("velocityUpperLimit", 3.5);
      properties.get("leftElbowPitch").put("effortLowerLimit", -65.0);
      properties.get("leftElbowPitch").put("effortUpperLimit", 65.0);
      properties.get("leftElbowPitch").put("kpPositionLimit", 100.0);
      properties.get("leftElbowPitch").put("kdPositionLimit", 20.0);
      properties.get("leftElbowPitch").put("kpVelocityLimit", 500.0);
      properties.get("leftElbowPitch").put("axis", new Vector3D(0.0, 0.0, 1.0));
      properties.get("leftElbowPitch").put("damping", 0.1);
      properties.get("leftElbowPitch").put("stiction", 0.0);
      properties.put("leftForearmYaw", new HashMap<>());
      properties.put("leftForearmLink", new HashMap<>());
      properties.get("leftForearmLink").put("mass", 2.476);
      properties.get("leftForearmLink").put("centerOfMass", new Vector3D(0.015, 0.13, 0.019564));
      properties.get("leftForearmLink")
                .put("inertia", new Matrix3D(0.0117554, 0.00130085, -7.27141E-4, 0.00130085, 0.00507157, 0.00169542, -7.27141E-4, 0.00169542, 0.0113657));
      properties.get("leftForearmYaw").put("offsetFromParentJoint", new Vector3D(-0.0254, 0.0, 0.0));
      properties.get("leftForearmYaw").put("positionLowerLimit", -2.019);
      properties.get("leftForearmYaw").put("positionUpperLimit", 3.14);
      properties.get("leftForearmYaw").put("velocityLowerLimit", -0.8);
      properties.get("leftForearmYaw").put("velocityUpperLimit", 0.8);
      properties.get("leftForearmYaw").put("effortLowerLimit", -26.0);
      properties.get("leftForearmYaw").put("effortUpperLimit", 26.0);
      properties.get("leftForearmYaw").put("kpPositionLimit", 100.0);
      properties.get("leftForearmYaw").put("kdPositionLimit", 20.0);
      properties.get("leftForearmYaw").put("kpVelocityLimit", 500.0);
      properties.get("leftForearmYaw").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("leftForearmYaw").put("damping", 0.1);
      properties.get("leftForearmYaw").put("stiction", 0.0);
      properties.put("leftWristRoll", new HashMap<>());
      properties.put("leftWristRollLink", new HashMap<>());
      properties.get("leftWristRollLink").put("mass", 0.14);
      properties.get("leftWristRollLink").put("centerOfMass", new Vector3D(-0.0, -0.0, -0.0));
      properties.get("leftWristRollLink").put("inertia", new Matrix3D(3.0251E-5, 1.25E-7, 3.6E-8, 1.25E-7, 3.772E-5, 0.0, 3.6E-8, 0.0, 9.395E-6));
      properties.get("leftWristRoll").put("offsetFromParentJoint", new Vector3D(0.0, 0.2871, 0.0));
      properties.get("leftWristRoll").put("positionLowerLimit", -0.35);
      properties.get("leftWristRoll").put("positionUpperLimit", 0.35);
      properties.get("leftWristRoll").put("velocityLowerLimit", -1.0);
      properties.get("leftWristRoll").put("velocityUpperLimit", 1.0);
      properties.get("leftWristRoll").put("effortLowerLimit", -14.0);
      properties.get("leftWristRoll").put("effortUpperLimit", 14.0);
      properties.get("leftWristRoll").put("kpPositionLimit", 100.0);
      properties.get("leftWristRoll").put("kdPositionLimit", 20.0);
      properties.get("leftWristRoll").put("kpVelocityLimit", 500.0);
      properties.get("leftWristRoll").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("leftWristRoll").put("damping", 0.1);
      properties.get("leftWristRoll").put("stiction", 0.0);
      properties.put("leftWristPitch", new HashMap<>());
      properties.put("leftPalm", new HashMap<>());
      properties.get("leftPalm").put("mass", 0.712);
      properties.get("leftPalm").put("centerOfMass", new Vector3D(0.002954, 0.052034, -2.36E-4));
      properties.get("leftPalm")
                .put("inertia", new Matrix3D(9.43493E-4, 3.4393E-5, 3.8828E-5, 3.4393E-5, 7.11024E-4, -2.3429E-5, 3.8828E-5, -2.3429E-5, 6.10199E-4));
      properties.get("leftWristPitch").put("offsetFromParentJoint", new Vector3D(0.0, 0.0, 0.0));
      properties.get("leftWristPitch").put("positionLowerLimit", -0.6);
      properties.get("leftWristPitch").put("positionUpperLimit", 0.6);
      properties.get("leftWristPitch").put("velocityLowerLimit", -1.0);
      properties.get("leftWristPitch").put("velocityUpperLimit", 1.0);
      properties.get("leftWristPitch").put("effortLowerLimit", -14.0);
      properties.get("leftWristPitch").put("effortUpperLimit", 14.0);
      properties.get("leftWristPitch").put("kpPositionLimit", 100.0);
      properties.get("leftWristPitch").put("kdPositionLimit", 20.0);
      properties.get("leftWristPitch").put("kpVelocityLimit", 500.0);
      properties.get("leftWristPitch").put("axis", new Vector3D(0.0, 0.0, 1.0));
      properties.get("leftWristPitch").put("damping", 0.1);
      properties.get("leftWristPitch").put("stiction", 0.0);
      properties.put("leftIndexFingerPitch1", new HashMap<>());
      properties.put("leftIndexFingerPitch1Link", new HashMap<>());
      properties.get("leftIndexFingerPitch1Link").put("mass", 0.02);
      properties.get("leftIndexFingerPitch1Link").put("centerOfMass", new Vector3D(0.0, 0.019696269397585765, 0.0034723150516228376));
      properties.get("leftIndexFingerPitch1Link")
                .put("inertia",
                     new Matrix3D(4.232E-6,
                                  5.769988217210815E-8,
                                  1.8295452913902818E-8,
                                  5.769988217210815E-8,
                                  1.2980849917342867E-6,
                                  -4.642891416145748E-7,
                                  1.8295452913902818E-8,
                                  -4.642891416145748E-7,
                                  3.8089150082657133E-6));
      properties.get("leftIndexFingerPitch1").put("offsetFromParentJoint", new Vector3D(0.0022000000000000006, 0.09760000000000002, 0.02350000000000002));
      properties.get("leftIndexFingerPitch1").put("positionLowerLimit", -1.57);
      properties.get("leftIndexFingerPitch1").put("positionUpperLimit", -0.0);
      properties.get("leftIndexFingerPitch1").put("velocityLowerLimit", -1.0);
      properties.get("leftIndexFingerPitch1").put("velocityUpperLimit", 1.0);
      properties.get("leftIndexFingerPitch1").put("effortLowerLimit", -10.0);
      properties.get("leftIndexFingerPitch1").put("effortUpperLimit", 10.0);
      properties.get("leftIndexFingerPitch1").put("kpPositionLimit", 100.0);
      properties.get("leftIndexFingerPitch1").put("kdPositionLimit", 20.0);
      properties.get("leftIndexFingerPitch1").put("kpVelocityLimit", 500.0);
      properties.get("leftIndexFingerPitch1").put("axis", new Vector3D(0.0, -0.17361607288187247, 0.9848134134124475));
      properties.get("leftIndexFingerPitch1").put("damping", 1.0);
      properties.get("leftIndexFingerPitch1").put("stiction", 0.0);
      properties.put("leftIndexFingerPitch2", new HashMap<>());
      properties.put("leftIndexFingerPitch2Link", new HashMap<>());
      properties.get("leftIndexFingerPitch2Link").put("mass", 0.018);
      properties.get("leftIndexFingerPitch2Link").put("centerOfMass", new Vector3D(0.0, 0.012802575108430745, 0.0022570047835548442));
      properties.get("leftIndexFingerPitch2Link")
                .put("inertia",
                     new Matrix3D(1.562E-6,
                                  1.4484824204393054E-8,
                                  1.575404290231016E-8,
                                  1.4484824204393054E-8,
                                  7.274652087801619E-7,
                                  -6.200070784124636E-8,
                                  1.575404290231016E-8,
                                  -6.200070784124639E-8,
                                  1.249534791219838E-6));
      properties.get("leftIndexFingerPitch2").put("offsetFromParentJoint", new Vector3D(0.0, 0.037519999999999935, 0.006614999999999957));
      properties.get("leftIndexFingerPitch2").put("positionLowerLimit", -1.658);
      properties.get("leftIndexFingerPitch2").put("positionUpperLimit", -0.0);
      properties.get("leftIndexFingerPitch2").put("velocityLowerLimit", -1.0);
      properties.get("leftIndexFingerPitch2").put("velocityUpperLimit", 1.0);
      properties.get("leftIndexFingerPitch2").put("effortLowerLimit", -10.0);
      properties.get("leftIndexFingerPitch2").put("effortUpperLimit", 10.0);
      properties.get("leftIndexFingerPitch2").put("kpPositionLimit", 100.0);
      properties.get("leftIndexFingerPitch2").put("kdPositionLimit", 20.0);
      properties.get("leftIndexFingerPitch2").put("kpVelocityLimit", 500.0);
      properties.get("leftIndexFingerPitch2").put("axis", new Vector3D(0.0, -0.17361607288187247, 0.9848134134124475));
      properties.get("leftIndexFingerPitch2").put("damping", 1.0);
      properties.get("leftIndexFingerPitch2").put("stiction", 0.0);
      properties.put("leftIndexFingerPitch3", new HashMap<>());
      properties.put("leftIndexFingerPitch3Link", new HashMap<>());
      properties.get("leftIndexFingerPitch3Link").put("mass", 0.01);
      properties.get("leftIndexFingerPitch3Link").put("centerOfMass", new Vector3D(0.0, 0.009848134698792883, 0.0017361575258114188));
      properties.get("leftIndexFingerPitch3Link")
                .put("inertia",
                     new Matrix3D(6.02E-7,
                                  2.3635523277102915E-8,
                                  4.1667780619474044E-9,
                                  2.3635523277102915E-8,
                                  2.1201844137782995E-7,
                                  -3.4121728199972275E-8,
                                  4.1667780619474044E-9,
                                  -3.412172819997229E-8,
                                  4.989815586221701E-7));
      properties.get("leftIndexFingerPitch3").put("offsetFromParentJoint", new Vector3D(0.0, 0.022550000000000042, 0.003976000000000067));
      properties.get("leftIndexFingerPitch3").put("positionLowerLimit", -1.92);
      properties.get("leftIndexFingerPitch3").put("positionUpperLimit", -0.0);
      properties.get("leftIndexFingerPitch3").put("velocityLowerLimit", -1.0);
      properties.get("leftIndexFingerPitch3").put("velocityUpperLimit", 1.0);
      properties.get("leftIndexFingerPitch3").put("effortLowerLimit", -10.0);
      properties.get("leftIndexFingerPitch3").put("effortUpperLimit", 10.0);
      properties.get("leftIndexFingerPitch3").put("kpPositionLimit", 100.0);
      properties.get("leftIndexFingerPitch3").put("kdPositionLimit", 20.0);
      properties.get("leftIndexFingerPitch3").put("kpVelocityLimit", 500.0);
      properties.get("leftIndexFingerPitch3").put("axis", new Vector3D(0.0, -0.17361607288187247, 0.9848134134124475));
      properties.get("leftIndexFingerPitch3").put("damping", 1.0);
      properties.get("leftIndexFingerPitch3").put("stiction", 0.0);
      properties.put("leftMiddleFingerPitch1", new HashMap<>());
      properties.put("leftMiddleFingerPitch1Link", new HashMap<>());
      properties.get("leftMiddleFingerPitch1Link").put("mass", 0.02);
      properties.get("leftMiddleFingerPitch1Link").put("centerOfMass", new Vector3D(0.0, 0.019850857332287003, -0.002437921896449382));
      properties.get("leftMiddleFingerPitch1Link")
                .put("inertia",
                     new Matrix3D(5.148E-6,
                                  4.3027657440849864E-8,
                                  5.798335550214148E-9,
                                  4.3027657440849864E-8,
                                  1.369619914645733E-6,
                                  3.91746684249759E-7,
                                  5.798335550214148E-9,
                                  3.9174668424975893E-7,
                                  4.544380085354267E-6));
      properties.get("leftMiddleFingerPitch1").put("offsetFromParentJoint", new Vector3D(0.0022000000000000006, 0.09699999999999998, -0.011899999999999966));
      properties.get("leftMiddleFingerPitch1").put("positionLowerLimit", -1.658);
      properties.get("leftMiddleFingerPitch1").put("positionUpperLimit", -0.0);
      properties.get("leftMiddleFingerPitch1").put("velocityLowerLimit", -1.0);
      properties.get("leftMiddleFingerPitch1").put("velocityUpperLimit", 1.0);
      properties.get("leftMiddleFingerPitch1").put("effortLowerLimit", -10.0);
      properties.get("leftMiddleFingerPitch1").put("effortUpperLimit", 10.0);
      properties.get("leftMiddleFingerPitch1").put("kpPositionLimit", 100.0);
      properties.get("leftMiddleFingerPitch1").put("kdPositionLimit", 20.0);
      properties.get("leftMiddleFingerPitch1").put("kpVelocityLimit", 500.0);
      properties.get("leftMiddleFingerPitch1").put("axis", new Vector3D(0.0, 0.12189598527100426, 0.9925428800685697));
      properties.get("leftMiddleFingerPitch1").put("damping", 1.0);
      properties.get("leftMiddleFingerPitch1").put("stiction", 0.0);
      properties.put("leftMiddleFingerPitch2", new HashMap<>());
      properties.put("leftMiddleFingerPitch2Link", new HashMap<>());
      properties.get("leftMiddleFingerPitch2Link").put("mass", 0.011);
      properties.get("leftMiddleFingerPitch2Link").put("centerOfMass", new Vector3D(0.0, 0.012903057265986551, -0.0015846492326920985));
      properties.get("leftMiddleFingerPitch2Link")
                .put("inertia",
                     new Matrix3D(1.97E-6,
                                  -3.240560042184074E-8,
                                  1.50624387567218E-8,
                                  -3.240560042184074E-8,
                                  5.606531868249063E-7,
                                  1.3217378737957825E-7,
                                  1.50624387567218E-8,
                                  1.321737873795783E-7,
                                  2.265346813175094E-6));
      properties.get("leftMiddleFingerPitch2").put("offsetFromParentJoint", new Vector3D(0.0, 0.037820000000000006, -0.004644000000000044));
      properties.get("leftMiddleFingerPitch2").put("positionLowerLimit", -1.92);
      properties.get("leftMiddleFingerPitch2").put("positionUpperLimit", -0.0);
      properties.get("leftMiddleFingerPitch2").put("velocityLowerLimit", -1.0);
      properties.get("leftMiddleFingerPitch2").put("velocityUpperLimit", 1.0);
      properties.get("leftMiddleFingerPitch2").put("effortLowerLimit", -10.0);
      properties.get("leftMiddleFingerPitch2").put("effortUpperLimit", 10.0);
      properties.get("leftMiddleFingerPitch2").put("kpPositionLimit", 100.0);
      properties.get("leftMiddleFingerPitch2").put("kdPositionLimit", 20.0);
      properties.get("leftMiddleFingerPitch2").put("kpVelocityLimit", 500.0);
      properties.get("leftMiddleFingerPitch2").put("axis", new Vector3D(0.0, 0.12189598527100426, 0.9925428800685697));
      properties.get("leftMiddleFingerPitch2").put("damping", 1.0);
      properties.get("leftMiddleFingerPitch2").put("stiction", 0.0);
      properties.put("leftMiddleFingerPitch3", new HashMap<>());
      properties.put("leftMiddleFingerPitch3Link", new HashMap<>());
      properties.get("leftMiddleFingerPitch3Link").put("mass", 0.006);
      properties.get("leftMiddleFingerPitch3Link").put("centerOfMass", new Vector3D(0.0, 0.009925428666143501, -0.001218960948224691));
      properties.get("leftMiddleFingerPitch3Link")
                .put("inertia",
                     new Matrix3D(3.96E-7,
                                  9.05478189435162E-9,
                                  -1.0452198678787192E-10,
                                  9.05478189435162E-9,
                                  1.659963752647085E-7,
                                  3.2825678158083905E-8,
                                  -1.0452198678787192E-10,
                                  3.282567815808391E-8,
                                  2.9700362473529145E-7));
      properties.get("leftMiddleFingerPitch3").put("offsetFromParentJoint", new Vector3D(0.0, 0.022730000000000167, -0.002791999999999949));
      properties.get("leftMiddleFingerPitch3").put("positionLowerLimit", -1.57);
      properties.get("leftMiddleFingerPitch3").put("positionUpperLimit", -0.0);
      properties.get("leftMiddleFingerPitch3").put("velocityLowerLimit", -1.0);
      properties.get("leftMiddleFingerPitch3").put("velocityUpperLimit", 1.0);
      properties.get("leftMiddleFingerPitch3").put("effortLowerLimit", -10.0);
      properties.get("leftMiddleFingerPitch3").put("effortUpperLimit", 10.0);
      properties.get("leftMiddleFingerPitch3").put("kpPositionLimit", 100.0);
      properties.get("leftMiddleFingerPitch3").put("kdPositionLimit", 20.0);
      properties.get("leftMiddleFingerPitch3").put("kpVelocityLimit", 500.0);
      properties.get("leftMiddleFingerPitch3").put("axis", new Vector3D(0.0, 0.12189598527100426, 0.9925428800685697));
      properties.get("leftMiddleFingerPitch3").put("damping", 1.0);
      properties.get("leftMiddleFingerPitch3").put("stiction", 0.0);
      properties.put("leftPinkyPitch1", new HashMap<>());
      properties.put("leftPinkyPitch1Link", new HashMap<>());
      properties.get("leftPinkyPitch1Link").put("mass", 0.02);
      properties.get("leftPinkyPitch1Link").put("centerOfMass", new Vector3D(0.0, 0.019850857332287003, -0.002437921896449382));
      properties.get("leftPinkyPitch1Link")
                .put("inertia",
                     new Matrix3D(5.148E-6,
                                  4.3027657440849864E-8,
                                  5.798335550214148E-9,
                                  4.3027657440849864E-8,
                                  1.369619914645733E-6,
                                  3.91746684249759E-7,
                                  5.798335550214148E-9,
                                  3.9174668424975893E-7,
                                  4.544380085354267E-6));
      properties.get("leftPinkyPitch1").put("offsetFromParentJoint", new Vector3D(0.0022000000000000006, 0.08379999999999999, -0.04099999999999998));
      properties.get("leftPinkyPitch1").put("positionLowerLimit", -1.57);
      properties.get("leftPinkyPitch1").put("positionUpperLimit", -0.0);
      properties.get("leftPinkyPitch1").put("velocityLowerLimit", -1.0);
      properties.get("leftPinkyPitch1").put("velocityUpperLimit", 1.0);
      properties.get("leftPinkyPitch1").put("effortLowerLimit", -10.0);
      properties.get("leftPinkyPitch1").put("effortUpperLimit", 10.0);
      properties.get("leftPinkyPitch1").put("kpPositionLimit", 100.0);
      properties.get("leftPinkyPitch1").put("kdPositionLimit", 20.0);
      properties.get("leftPinkyPitch1").put("kpVelocityLimit", 500.0);
      properties.get("leftPinkyPitch1").put("axis", new Vector3D(0.0, 0.12189598527100426, 0.9925428800685697));
      properties.get("leftPinkyPitch1").put("damping", 1.0);
      properties.get("leftPinkyPitch1").put("stiction", 0.0);
      properties.put("leftPinkyPitch2", new HashMap<>());
      properties.put("leftPinkyPitch2Link", new HashMap<>());
      properties.get("leftPinkyPitch2Link").put("mass", 0.011);
      properties.get("leftPinkyPitch2Link").put("centerOfMass", new Vector3D(0.0, 0.012903057265986551, -0.0015846492326920985));
      properties.get("leftPinkyPitch2Link")
                .put("inertia",
                     new Matrix3D(1.97E-6,
                                  -3.240560042184074E-8,
                                  1.50624387567218E-8,
                                  -3.240560042184074E-8,
                                  5.606531868249063E-7,
                                  1.3217378737957825E-7,
                                  1.50624387567218E-8,
                                  1.321737873795783E-7,
                                  2.265346813175094E-6));
      properties.get("leftPinkyPitch2").put("offsetFromParentJoint", new Vector3D(0.0, 0.037816000000000016, -0.0046439999999999025));
      properties.get("leftPinkyPitch2").put("positionLowerLimit", -1.658);
      properties.get("leftPinkyPitch2").put("positionUpperLimit", -0.0);
      properties.get("leftPinkyPitch2").put("velocityLowerLimit", -1.0);
      properties.get("leftPinkyPitch2").put("velocityUpperLimit", 1.0);
      properties.get("leftPinkyPitch2").put("effortLowerLimit", -10.0);
      properties.get("leftPinkyPitch2").put("effortUpperLimit", 10.0);
      properties.get("leftPinkyPitch2").put("kpPositionLimit", 100.0);
      properties.get("leftPinkyPitch2").put("kdPositionLimit", 20.0);
      properties.get("leftPinkyPitch2").put("kpVelocityLimit", 500.0);
      properties.get("leftPinkyPitch2").put("axis", new Vector3D(0.0, 0.12189598527100426, 0.9925428800685697));
      properties.get("leftPinkyPitch2").put("damping", 1.0);
      properties.get("leftPinkyPitch2").put("stiction", 0.0);
      properties.put("leftPinkyPitch3", new HashMap<>());
      properties.put("leftPinkyPitch3Link", new HashMap<>());
      properties.get("leftPinkyPitch3Link").put("mass", 0.006);
      properties.get("leftPinkyPitch3Link").put("centerOfMass", new Vector3D(0.0, 0.009925428666143501, -0.001218960948224691));
      properties.get("leftPinkyPitch3Link")
                .put("inertia",
                     new Matrix3D(3.96E-7,
                                  9.05478189435162E-9,
                                  -1.0452198678787192E-10,
                                  9.05478189435162E-9,
                                  1.659963752647085E-7,
                                  3.2825678158083905E-8,
                                  -1.0452198678787192E-10,
                                  3.282567815808391E-8,
                                  2.9700362473529145E-7));
      properties.get("leftPinkyPitch3").put("offsetFromParentJoint", new Vector3D(0.0, 0.022734000000000056, -0.0027920000000000765));
      properties.get("leftPinkyPitch3").put("positionLowerLimit", -1.92);
      properties.get("leftPinkyPitch3").put("positionUpperLimit", -0.0);
      properties.get("leftPinkyPitch3").put("velocityLowerLimit", -1.0);
      properties.get("leftPinkyPitch3").put("velocityUpperLimit", 1.0);
      properties.get("leftPinkyPitch3").put("effortLowerLimit", -10.0);
      properties.get("leftPinkyPitch3").put("effortUpperLimit", 10.0);
      properties.get("leftPinkyPitch3").put("kpPositionLimit", 100.0);
      properties.get("leftPinkyPitch3").put("kdPositionLimit", 20.0);
      properties.get("leftPinkyPitch3").put("kpVelocityLimit", 500.0);
      properties.get("leftPinkyPitch3").put("axis", new Vector3D(0.0, 0.12189598527100426, 0.9925428800685697));
      properties.get("leftPinkyPitch3").put("damping", 1.0);
      properties.get("leftPinkyPitch3").put("stiction", 0.0);
      properties.put("leftThumbRoll", new HashMap<>());
      properties.put("leftThumbRollLink", new HashMap<>());
      properties.get("leftThumbRollLink").put("mass", 0.017);
      properties.get("leftThumbRollLink").put("centerOfMass", new Vector3D(0.0, 0.003420522332544199, 0.009396809403865038));
      properties.get("leftThumbRollLink")
                .put("inertia",
                     new Matrix3D(2.77788E-6,
                                  3.800365322933979E-8,
                                  8.7821261445544E-8,
                                  3.800365322933979E-8,
                                  3.2663699259248467E-6,
                                  -7.322727571414368E-7,
                                  8.7821261445544E-8,
                                  -7.322727571414366E-7,
                                  1.6352200740751536E-6));
      properties.get("leftThumbRoll").put("offsetFromParentJoint", new Vector3D(0.0049, 0.03510000000000002, 0.022800000000000042));
      properties.get("leftThumbRoll").put("positionLowerLimit", 0.0);
      properties.get("leftThumbRoll").put("positionUpperLimit", 2.356);
      properties.get("leftThumbRoll").put("velocityLowerLimit", -1.0);
      properties.get("leftThumbRoll").put("velocityUpperLimit", 1.0);
      properties.get("leftThumbRoll").put("effortLowerLimit", -10.0);
      properties.get("leftThumbRoll").put("effortUpperLimit", 10.0);
      properties.get("leftThumbRoll").put("kpPositionLimit", 100.0);
      properties.get("leftThumbRoll").put("kdPositionLimit", 20.0);
      properties.get("leftThumbRoll").put("kpVelocityLimit", 500.0);
      properties.get("leftThumbRoll").put("axis", new Vector3D(0.0, 0.939681022333869, -0.34205200812972125));
      properties.get("leftThumbRoll").put("damping", 1.0);
      properties.get("leftThumbRoll").put("stiction", 0.0);
      properties.put("leftThumbPitch1", new HashMap<>());
      properties.put("leftThumbPitch1Link", new HashMap<>());
      properties.get("leftThumbPitch1Link").put("mass", 0.02);
      properties.get("leftThumbPitch1Link").put("centerOfMass", new Vector3D(0.0, 0.005, 0.02));
      properties.get("leftThumbPitch1Link").put("inertia", new Matrix3D(4.239E-6, 0.0, 0.0, 0.0, 4.582E-6, -0.0, 0.0, -0.0, 1.47E-6));
      properties.get("leftThumbPitch1").put("offsetFromParentJoint", new Vector3D(0.0, 0.007832999999999965, 0.021518999999999993));
      properties.get("leftThumbPitch1").put("positionLowerLimit", -1.658);
      properties.get("leftThumbPitch1").put("positionUpperLimit", -0.0);
      properties.get("leftThumbPitch1").put("velocityLowerLimit", -1.0);
      properties.get("leftThumbPitch1").put("velocityUpperLimit", 1.0);
      properties.get("leftThumbPitch1").put("effortLowerLimit", -10.0);
      properties.get("leftThumbPitch1").put("effortUpperLimit", 10.0);
      properties.get("leftThumbPitch1").put("kpPositionLimit", 100.0);
      properties.get("leftThumbPitch1").put("kdPositionLimit", 20.0);
      properties.get("leftThumbPitch1").put("kpVelocityLimit", 500.0);
      properties.get("leftThumbPitch1").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("leftThumbPitch1").put("damping", 1.0);
      properties.get("leftThumbPitch1").put("stiction", 0.0);
      properties.put("leftThumbPitch2", new HashMap<>());
      properties.put("leftThumbPitch2Link", new HashMap<>());
      properties.get("leftThumbPitch2Link").put("mass", 0.013);
      properties.get("leftThumbPitch2Link").put("centerOfMass", new Vector3D(0.0, 0.004, 0.017));
      properties.get("leftThumbPitch2Link").put("inertia", new Matrix3D(1.266E-6, 0.0, 0.0, 0.0, 1.503E-6, 0.0, 0.0, 0.0, 6.99E-7));
      properties.get("leftThumbPitch2").put("offsetFromParentJoint", new Vector3D(0.0, 0.00660000000000005, 0.03750000000000003));
      properties.get("leftThumbPitch2").put("positionLowerLimit", -1.92);
      properties.get("leftThumbPitch2").put("positionUpperLimit", -0.0);
      properties.get("leftThumbPitch2").put("velocityLowerLimit", -1.0);
      properties.get("leftThumbPitch2").put("velocityUpperLimit", 1.0);
      properties.get("leftThumbPitch2").put("effortLowerLimit", -10.0);
      properties.get("leftThumbPitch2").put("effortUpperLimit", 10.0);
      properties.get("leftThumbPitch2").put("kpPositionLimit", 100.0);
      properties.get("leftThumbPitch2").put("kdPositionLimit", 20.0);
      properties.get("leftThumbPitch2").put("kpVelocityLimit", 500.0);
      properties.get("leftThumbPitch2").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("leftThumbPitch2").put("damping", 1.0);
      properties.get("leftThumbPitch2").put("stiction", 0.0);
      properties.put("leftThumbPitch3", new HashMap<>());
      properties.put("leftThumbPitch3Link", new HashMap<>());
      properties.get("leftThumbPitch3Link").put("mass", 0.006);
      properties.get("leftThumbPitch3Link").put("centerOfMass", new Vector3D(0.0, 0.004, 0.01));
      properties.get("leftThumbPitch3Link").put("inertia", new Matrix3D(3.22E-7, 0.0, 0.0, 0.0, 4.06E-7, 0.0, 0.0, 0.0, 2.1E-7));
      properties.get("leftThumbPitch3").put("offsetFromParentJoint", new Vector3D(0.0, 0.004899999999999904, 0.02749999999999997));
      properties.get("leftThumbPitch3").put("positionLowerLimit", -1.57);
      properties.get("leftThumbPitch3").put("positionUpperLimit", -0.0);
      properties.get("leftThumbPitch3").put("velocityLowerLimit", -1.0);
      properties.get("leftThumbPitch3").put("velocityUpperLimit", 1.0);
      properties.get("leftThumbPitch3").put("effortLowerLimit", -10.0);
      properties.get("leftThumbPitch3").put("effortUpperLimit", 10.0);
      properties.get("leftThumbPitch3").put("kpPositionLimit", 100.0);
      properties.get("leftThumbPitch3").put("kdPositionLimit", 20.0);
      properties.get("leftThumbPitch3").put("kpVelocityLimit", 500.0);
      properties.get("leftThumbPitch3").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("leftThumbPitch3").put("damping", 1.0);
      properties.get("leftThumbPitch3").put("stiction", 0.0);
      properties.put("lowerNeckPitch", new HashMap<>());
      properties.put("lowerNeckPitchLink", new HashMap<>());
      properties.get("lowerNeckPitchLink").put("mass", 1.05);
      properties.get("lowerNeckPitchLink").put("centerOfMass", new Vector3D(-0.02, 0.0, 0.04));
      properties.get("lowerNeckPitchLink")
                .put("inertia", new Matrix3D(0.00147072, -1.26021E-4, 6.32521E-4, -1.26021E-4, 0.00185192, 2.43012E-4, 6.32521E-4, 2.43012E-4, 8.32117E-4));
      properties.get("lowerNeckPitch").put("offsetFromParentJoint", new Vector3D(0.020351799999999996, 0.0, 0.33845000000000003));
      properties.get("lowerNeckPitch").put("positionLowerLimit", 0.0);
      properties.get("lowerNeckPitch").put("positionUpperLimit", 1.162);
      properties.get("lowerNeckPitch").put("velocityLowerLimit", -5.0);
      properties.get("lowerNeckPitch").put("velocityUpperLimit", 5.0);
      properties.get("lowerNeckPitch").put("effortLowerLimit", -26.0);
      properties.get("lowerNeckPitch").put("effortUpperLimit", 26.0);
      properties.get("lowerNeckPitch").put("kpPositionLimit", 100.0);
      properties.get("lowerNeckPitch").put("kdPositionLimit", 20.0);
      properties.get("lowerNeckPitch").put("kpVelocityLimit", 500.0);
      properties.get("lowerNeckPitch").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("lowerNeckPitch").put("damping", 0.1);
      properties.get("lowerNeckPitch").put("stiction", 0.0);
      properties.put("neckYaw", new HashMap<>());
      properties.put("neckYawLink", new HashMap<>());
      properties.get("neckYawLink").put("mass", 1.4);
      properties.get("neckYawLink").put("centerOfMass", new Vector3D(-0.03, -0.01, 0.15));
      properties.get("neckYawLink")
                .put("inertia", new Matrix3D(0.0019977, -1.80062E-4, 7.23677E-4, -1.80062E-4, 0.00291993, 2.46467E-4, 7.23677E-4, 2.46467E-4, 0.00211975));
      properties.get("neckYaw").put("offsetFromParentJoint", new Vector3D(-0.051924, 0.0, 0.0));
      properties.get("neckYaw").put("positionLowerLimit", -1.0472);
      properties.get("neckYaw").put("positionUpperLimit", 1.0472);
      properties.get("neckYaw").put("velocityLowerLimit", -5.0);
      properties.get("neckYaw").put("velocityUpperLimit", 5.0);
      properties.get("neckYaw").put("effortLowerLimit", -26.0);
      properties.get("neckYaw").put("effortUpperLimit", 26.0);
      properties.get("neckYaw").put("kpPositionLimit", 100.0);
      properties.get("neckYaw").put("kdPositionLimit", 20.0);
      properties.get("neckYaw").put("kpVelocityLimit", 500.0);
      properties.get("neckYaw").put("axis", new Vector3D(0.0, 0.0, 1.0));
      properties.get("neckYaw").put("damping", 0.1);
      properties.get("neckYaw").put("stiction", 0.0);
      properties.put("upperNeckPitch", new HashMap<>());
      properties.put("upperNeckPitchLink", new HashMap<>());
      properties.get("upperNeckPitchLink").put("mass", 5.30991);
      properties.get("upperNeckPitchLink").put("centerOfMass", new Vector3D(0.102036, 0.00422754, 0.0409152));
      properties.get("upperNeckPitchLink")
                .put("inertia", new Matrix3D(0.0446288, -4.43568E-4, -0.0021252, -4.43568E-4, 0.0461084, -6.44772E-4, -0.0021252, -6.44772E-4, 0.0402031));
      properties.get("upperNeckPitch").put("offsetFromParentJoint", new Vector3D(-0.06, 0.0, 0.19599699999999998));
      properties.get("upperNeckPitch").put("positionLowerLimit", -0.872);
      properties.get("upperNeckPitch").put("positionUpperLimit", 0.0);
      properties.get("upperNeckPitch").put("velocityLowerLimit", -5.0);
      properties.get("upperNeckPitch").put("velocityUpperLimit", 5.0);
      properties.get("upperNeckPitch").put("effortLowerLimit", -26.0);
      properties.get("upperNeckPitch").put("effortUpperLimit", 26.0);
      properties.get("upperNeckPitch").put("kpPositionLimit", 100.0);
      properties.get("upperNeckPitch").put("kdPositionLimit", 20.0);
      properties.get("upperNeckPitch").put("kpVelocityLimit", 500.0);
      properties.get("upperNeckPitch").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("upperNeckPitch").put("damping", 0.1);
      properties.get("upperNeckPitch").put("stiction", 0.0);
      properties.put("hokuyo_joint", new HashMap<>());
      properties.put("hokuyo_link", new HashMap<>());
      properties.get("hokuyo_link").put("mass", 0.057664);
      properties.get("hokuyo_link").put("centerOfMass", new Vector3D(0.03270279235379761, -4.084110874927957E-4, -9.106101256784079E-5));
      properties.get("hokuyo_link")
                .put("inertia",
                     new Matrix3D(3.541192586390836E-5,
                                  -5.075197116557559E-8,
                                  -1.004534784646333E-5,
                                  -5.075197116557559E-8,
                                  4.343700005209264E-5,
                                  -3.2175569823047584E-9,
                                  -1.0045347846463328E-5,
                                  -3.217556982304757E-9,
                                  4.545707408399899E-5));
      properties.get("hokuyo_joint").put("offsetFromParentJoint", new Vector3D(0.1278812, 2.33516E-7, -0.006071999999999966));
      properties.get("hokuyo_joint").put("positionLowerLimit", -1.0E16);
      properties.get("hokuyo_joint").put("positionUpperLimit", 1.0E16);
      properties.get("hokuyo_joint").put("velocityLowerLimit", -Infinity);
      properties.get("hokuyo_joint").put("velocityUpperLimit", Infinity);
      properties.get("hokuyo_joint").put("effortLowerLimit", -Infinity);
      properties.get("hokuyo_joint").put("effortUpperLimit", Infinity);
      properties.get("hokuyo_joint").put("kpPositionLimit", 100.0);
      properties.get("hokuyo_joint").put("kdPositionLimit", 20.0);
      properties.get("hokuyo_joint").put("kpVelocityLimit", 0.0);
      properties.get("hokuyo_joint").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("hokuyo_joint").put("damping", 0.01);
      properties.get("hokuyo_joint").put("stiction", 0.0);
      properties.put("rightShoulderPitch", new HashMap<>());
      properties.put("rightShoulderPitchLink", new HashMap<>());
      properties.get("rightShoulderPitchLink").put("mass", 2.65);
      properties.get("rightShoulderPitchLink").put("centerOfMass", new Vector3D(0.012, -0.251, 0.0));
      properties.get("rightShoulderPitchLink").put("inertia", new Matrix3D(0.0137182, 0.0, 0.0, 0.0, 0.0105028, 0.0, 0.0, 0.0, 0.0148064));
      properties.get("rightShoulderPitch").put("offsetFromParentJoint", new Vector3D(-0.0316, 0.0, 0.2984));
      properties.get("rightShoulderPitch").put("positionLowerLimit", -2.85);
      properties.get("rightShoulderPitch").put("positionUpperLimit", 2.0);
      properties.get("rightShoulderPitch").put("velocityLowerLimit", -3.0);
      properties.get("rightShoulderPitch").put("velocityUpperLimit", 3.0);
      properties.get("rightShoulderPitch").put("effortLowerLimit", -190.0);
      properties.get("rightShoulderPitch").put("effortUpperLimit", 190.0);
      properties.get("rightShoulderPitch").put("kpPositionLimit", 100.0);
      properties.get("rightShoulderPitch").put("kdPositionLimit", 20.0);
      properties.get("rightShoulderPitch").put("kpVelocityLimit", 500.0);
      properties.get("rightShoulderPitch").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("rightShoulderPitch").put("damping", 0.1);
      properties.get("rightShoulderPitch").put("stiction", 0.0);
      properties.put("rightShoulderRoll", new HashMap<>());
      properties.put("rightShoulderRollLink", new HashMap<>());
      properties.get("rightShoulderRollLink").put("mass", 3.97);
      properties.get("rightShoulderRollLink").put("centerOfMass", new Vector3D(0.008513, -0.02068, -0.001088));
      properties.get("rightShoulderRollLink")
                .put("inertia", new Matrix3D(0.0145988, -6.6764E-4, 3.629E-5, -6.6764E-4, 0.00645214, 8.283E-5, 3.629E-5, 8.283E-5, 0.0168483));
      properties.get("rightShoulderRoll").put("offsetFromParentJoint", new Vector3D(0.0, -0.2499, 0.0));
      properties.get("rightShoulderRoll").put("positionLowerLimit", -1.266);
      properties.get("rightShoulderRoll").put("positionUpperLimit", 1.519);
      properties.get("rightShoulderRoll").put("velocityLowerLimit", -3.5);
      properties.get("rightShoulderRoll").put("velocityUpperLimit", 3.5);
      properties.get("rightShoulderRoll").put("effortLowerLimit", -350.0);
      properties.get("rightShoulderRoll").put("effortUpperLimit", 350.0);
      properties.get("rightShoulderRoll").put("kpPositionLimit", 100.0);
      properties.get("rightShoulderRoll").put("kdPositionLimit", 20.0);
      properties.get("rightShoulderRoll").put("kpVelocityLimit", 500.0);
      properties.get("rightShoulderRoll").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("rightShoulderRoll").put("damping", 0.1);
      properties.get("rightShoulderRoll").put("stiction", 0.0);
      properties.put("rightShoulderYaw", new HashMap<>());
      properties.put("rightShoulderYawLink", new HashMap<>());
      properties.get("rightShoulderYawLink").put("mass", 3.085);
      properties.get("rightShoulderYawLink").put("centerOfMass", new Vector3D(-0.004304, -0.209832, -0.007295));
      properties.get("rightShoulderYawLink")
                .put("inertia", new Matrix3D(0.0393552, 0.00782708, 7.53947E-4, 0.00782708, 0.00490577, -0.00272387, 7.53947E-4, -0.00272387, 0.0418795));
      properties.get("rightShoulderYaw").put("offsetFromParentJoint", new Vector3D(0.0, 0.0, 0.0));
      properties.get("rightShoulderYaw").put("positionLowerLimit", -3.1);
      properties.get("rightShoulderYaw").put("positionUpperLimit", 2.18);
      properties.get("rightShoulderYaw").put("velocityLowerLimit", -1.5);
      properties.get("rightShoulderYaw").put("velocityUpperLimit", 1.5);
      properties.get("rightShoulderYaw").put("effortLowerLimit", -65.0);
      properties.get("rightShoulderYaw").put("effortUpperLimit", 65.0);
      properties.get("rightShoulderYaw").put("kpPositionLimit", 100.0);
      properties.get("rightShoulderYaw").put("kdPositionLimit", 20.0);
      properties.get("rightShoulderYaw").put("kpVelocityLimit", 500.0);
      properties.get("rightShoulderYaw").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("rightShoulderYaw").put("damping", 0.1);
      properties.get("rightShoulderYaw").put("stiction", 0.0);
      properties.put("rightElbowPitch", new HashMap<>());
      properties.put("rightElbowPitchLink", new HashMap<>());
      properties.get("rightElbowPitchLink").put("mass", 1.83);
      properties.get("rightElbowPitchLink").put("centerOfMass", new Vector3D(-0.020344, -0.014722, -0.0223));
      properties.get("rightElbowPitchLink")
                .put("inertia", new Matrix3D(0.00331452, -5.35099E-4, -7.28077E-4, -5.35099E-4, 0.00350567, -4.23865E-4, -7.28077E-4, -4.23865E-4, 0.00301128));
      properties.get("rightElbowPitch").put("offsetFromParentJoint", new Vector3D(0.0254, -0.32999999999999996, 0.0));
      properties.get("rightElbowPitch").put("positionLowerLimit", -0.12);
      properties.get("rightElbowPitch").put("positionUpperLimit", 2.174);
      properties.get("rightElbowPitch").put("velocityLowerLimit", -3.5);
      properties.get("rightElbowPitch").put("velocityUpperLimit", 3.5);
      properties.get("rightElbowPitch").put("effortLowerLimit", -65.0);
      properties.get("rightElbowPitch").put("effortUpperLimit", 65.0);
      properties.get("rightElbowPitch").put("kpPositionLimit", 100.0);
      properties.get("rightElbowPitch").put("kdPositionLimit", 20.0);
      properties.get("rightElbowPitch").put("kpVelocityLimit", 500.0);
      properties.get("rightElbowPitch").put("axis", new Vector3D(0.0, 0.0, 1.0));
      properties.get("rightElbowPitch").put("damping", 0.1);
      properties.get("rightElbowPitch").put("stiction", 0.0);
      properties.put("rightForearmYaw", new HashMap<>());
      properties.put("rightForearmLink", new HashMap<>());
      properties.get("rightForearmLink").put("mass", 2.476);
      properties.get("rightForearmLink").put("centerOfMass", new Vector3D(0.015, -0.13, 0.019564));
      properties.get("rightForearmLink")
                .put("inertia", new Matrix3D(0.0117554, -0.00130085, -7.27141E-4, -0.00130085, 0.00507157, -0.00169542, -7.27141E-4, -0.00169542, 0.0113657));
      properties.get("rightForearmYaw").put("offsetFromParentJoint", new Vector3D(-0.0254, 0.0, 0.0));
      properties.get("rightForearmYaw").put("positionLowerLimit", -2.019);
      properties.get("rightForearmYaw").put("positionUpperLimit", 3.14);
      properties.get("rightForearmYaw").put("velocityLowerLimit", -0.8);
      properties.get("rightForearmYaw").put("velocityUpperLimit", 0.8);
      properties.get("rightForearmYaw").put("effortLowerLimit", -26.0);
      properties.get("rightForearmYaw").put("effortUpperLimit", 26.0);
      properties.get("rightForearmYaw").put("kpPositionLimit", 100.0);
      properties.get("rightForearmYaw").put("kdPositionLimit", 20.0);
      properties.get("rightForearmYaw").put("kpVelocityLimit", 500.0);
      properties.get("rightForearmYaw").put("axis", new Vector3D(0.0, 1.0, 0.0));
      properties.get("rightForearmYaw").put("damping", 0.1);
      properties.get("rightForearmYaw").put("stiction", 0.0);
      properties.put("rightWristRoll", new HashMap<>());
      properties.put("rightWristRollLink", new HashMap<>());
      properties.get("rightWristRollLink").put("mass", 0.14);
      properties.get("rightWristRollLink").put("centerOfMass", new Vector3D(-0.0, -0.0, -0.0));
      properties.get("rightWristRollLink").put("inertia", new Matrix3D(3.0251E-5, -1.25E-7, -3.6E-8, -1.25E-7, 3.772E-5, 0.0, -3.6E-8, 0.0, 9.395E-6));
      properties.get("rightWristRoll").put("offsetFromParentJoint", new Vector3D(0.0, -0.2871, 0.0));
      properties.get("rightWristRoll").put("positionLowerLimit", -0.35);
      properties.get("rightWristRoll").put("positionUpperLimit", 0.35);
      properties.get("rightWristRoll").put("velocityLowerLimit", -1.0);
      properties.get("rightWristRoll").put("velocityUpperLimit", 1.0);
      properties.get("rightWristRoll").put("effortLowerLimit", -14.0);
      properties.get("rightWristRoll").put("effortUpperLimit", 14.0);
      properties.get("rightWristRoll").put("kpPositionLimit", 100.0);
      properties.get("rightWristRoll").put("kdPositionLimit", 20.0);
      properties.get("rightWristRoll").put("kpVelocityLimit", 500.0);
      properties.get("rightWristRoll").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("rightWristRoll").put("damping", 0.1);
      properties.get("rightWristRoll").put("stiction", 0.0);
      properties.put("rightWristPitch", new HashMap<>());
      properties.put("rightPalm", new HashMap<>());
      properties.get("rightPalm").put("mass", 0.712);
      properties.get("rightPalm").put("centerOfMass", new Vector3D(0.002954, -0.052034, -2.36E-4));
      properties.get("rightPalm")
                .put("inertia", new Matrix3D(9.43493E-4, 3.4393E-5, -3.8828E-5, 3.4393E-5, 7.11024E-4, 2.3429E-5, -3.8828E-5, 2.3429E-5, 6.10199E-4));
      properties.get("rightWristPitch").put("offsetFromParentJoint", new Vector3D(0.0, 0.0, 0.0));
      properties.get("rightWristPitch").put("positionLowerLimit", -0.6);
      properties.get("rightWristPitch").put("positionUpperLimit", 0.6);
      properties.get("rightWristPitch").put("velocityLowerLimit", -1.0);
      properties.get("rightWristPitch").put("velocityUpperLimit", 1.0);
      properties.get("rightWristPitch").put("effortLowerLimit", -14.0);
      properties.get("rightWristPitch").put("effortUpperLimit", 14.0);
      properties.get("rightWristPitch").put("kpPositionLimit", 100.0);
      properties.get("rightWristPitch").put("kdPositionLimit", 20.0);
      properties.get("rightWristPitch").put("kpVelocityLimit", 500.0);
      properties.get("rightWristPitch").put("axis", new Vector3D(0.0, 0.0, 1.0));
      properties.get("rightWristPitch").put("damping", 0.1);
      properties.get("rightWristPitch").put("stiction", 0.0);
      properties.put("rightIndexFingerPitch1", new HashMap<>());
      properties.put("rightIndexFingerPitch1Link", new HashMap<>());
      properties.get("rightIndexFingerPitch1Link").put("mass", 0.02);
      properties.get("rightIndexFingerPitch1Link").put("centerOfMass", new Vector3D(0.0, -0.019696269397585765, 0.0034723150516228376));
      properties.get("rightIndexFingerPitch1Link")
                .put("inertia",
                     new Matrix3D(4.232E-6,
                                  -6.047773421340643E-8,
                                  2.5384373958342057E-9,
                                  -6.047773421340643E-8,
                                  1.2932975760459825E-6,
                                  4.511331296418157E-7,
                                  2.5384373958342057E-9,
                                  4.511331296418157E-7,
                                  3.813702423954017E-6));
      properties.get("rightIndexFingerPitch1").put("offsetFromParentJoint", new Vector3D(0.0022000000000000006, -0.09760000000000002, 0.02350000000000002));
      properties.get("rightIndexFingerPitch1").put("positionLowerLimit", 0.0);
      properties.get("rightIndexFingerPitch1").put("positionUpperLimit", 1.57);
      properties.get("rightIndexFingerPitch1").put("velocityLowerLimit", -1.0);
      properties.get("rightIndexFingerPitch1").put("velocityUpperLimit", 1.0);
      properties.get("rightIndexFingerPitch1").put("effortLowerLimit", -10.0);
      properties.get("rightIndexFingerPitch1").put("effortUpperLimit", 10.0);
      properties.get("rightIndexFingerPitch1").put("kpPositionLimit", 100.0);
      properties.get("rightIndexFingerPitch1").put("kdPositionLimit", 20.0);
      properties.get("rightIndexFingerPitch1").put("kpVelocityLimit", 500.0);
      properties.get("rightIndexFingerPitch1").put("axis", new Vector3D(0.0, 0.17361607288187247, 0.9848134134124475));
      properties.get("rightIndexFingerPitch1").put("damping", 1.0);
      properties.get("rightIndexFingerPitch1").put("stiction", 0.0);
      properties.put("rightIndexFingerPitch2", new HashMap<>());
      properties.put("rightIndexFingerPitch2Link", new HashMap<>());
      properties.get("rightIndexFingerPitch2Link").put("mass", 0.018);
      properties.get("rightIndexFingerPitch2Link").put("centerOfMass", new Vector3D(0.0, -0.012802575108430745, 0.0022570047835548442));
      properties.get("rightIndexFingerPitch2Link")
                .put("inertia",
                     new Matrix3D(1.562E-6,
                                  -1.8998833771502745E-8,
                                  -9.851107314551334E-9,
                                  -1.8998833771502745E-8,
                                  7.486666211140791E-7,
                                  1.2026304657775112E-7,
                                  -9.851107314551334E-9,
                                  1.2026304657775112E-7,
                                  1.2283333788859208E-6));
      properties.get("rightIndexFingerPitch2").put("offsetFromParentJoint", new Vector3D(0.0, -0.037519999999999935, 0.006614999999999957));
      properties.get("rightIndexFingerPitch2").put("positionLowerLimit", 0.0);
      properties.get("rightIndexFingerPitch2").put("positionUpperLimit", 1.658);
      properties.get("rightIndexFingerPitch2").put("velocityLowerLimit", -1.0);
      properties.get("rightIndexFingerPitch2").put("velocityUpperLimit", 1.0);
      properties.get("rightIndexFingerPitch2").put("effortLowerLimit", -10.0);
      properties.get("rightIndexFingerPitch2").put("effortUpperLimit", 10.0);
      properties.get("rightIndexFingerPitch2").put("kpPositionLimit", 100.0);
      properties.get("rightIndexFingerPitch2").put("kdPositionLimit", 20.0);
      properties.get("rightIndexFingerPitch2").put("kpVelocityLimit", 500.0);
      properties.get("rightIndexFingerPitch2").put("axis", new Vector3D(0.0, 0.17361607288187247, 0.9848134134124475));
      properties.get("rightIndexFingerPitch2").put("damping", 1.0);
      properties.get("rightIndexFingerPitch2").put("stiction", 0.0);
      properties.put("rightIndexFingerPitch3", new HashMap<>());
      properties.put("rightIndexFingerPitch3Link", new HashMap<>());
      properties.get("rightIndexFingerPitch3Link").put("mass", 0.01);
      properties.get("rightIndexFingerPitch3Link").put("centerOfMass", new Vector3D(0.0, -0.009848134698792883, 0.0017361575258114188));
      properties.get("rightIndexFingerPitch3Link")
                .put("inertia",
                     new Matrix3D(6.02E-7,
                                  -2.3635523277102915E-8,
                                  4.1667780619474044E-9,
                                  -2.3635523277102915E-8,
                                  2.2364502233513937E-7,
                                  6.607204299095878E-8,
                                  4.1667780619474044E-9,
                                  6.607204299095878E-8,
                                  4.873549776648605E-7));
      properties.get("rightIndexFingerPitch3").put("offsetFromParentJoint", new Vector3D(0.0, -0.022550000000000042, 0.003976000000000067));
      properties.get("rightIndexFingerPitch3").put("positionLowerLimit", 0.0);
      properties.get("rightIndexFingerPitch3").put("positionUpperLimit", 1.92);
      properties.get("rightIndexFingerPitch3").put("velocityLowerLimit", -1.0);
      properties.get("rightIndexFingerPitch3").put("velocityUpperLimit", 1.0);
      properties.get("rightIndexFingerPitch3").put("effortLowerLimit", -10.0);
      properties.get("rightIndexFingerPitch3").put("effortUpperLimit", 10.0);
      properties.get("rightIndexFingerPitch3").put("kpPositionLimit", 100.0);
      properties.get("rightIndexFingerPitch3").put("kdPositionLimit", 20.0);
      properties.get("rightIndexFingerPitch3").put("kpVelocityLimit", 500.0);
      properties.get("rightIndexFingerPitch3").put("axis", new Vector3D(0.0, 0.17361607288187247, 0.9848134134124475));
      properties.get("rightIndexFingerPitch3").put("damping", 1.0);
      properties.get("rightIndexFingerPitch3").put("stiction", 0.0);
      properties.put("rightMiddleFingerPitch1", new HashMap<>());
      properties.put("rightMiddleFingerPitch1Link", new HashMap<>());
      properties.get("rightMiddleFingerPitch1Link").put("mass", 0.02);
      properties.get("rightMiddleFingerPitch1Link").put("centerOfMass", new Vector3D(0.0, -0.019850857332287003, -0.002437921896449382));
      properties.get("rightMiddleFingerPitch1Link")
                .put("inertia",
                     new Matrix3D(5.148E-6,
                                  -4.034594335475554E-8,
                                  -1.6037607515301554E-8,
                                  -4.034594335475554E-8,
                                  1.37155570823588E-6,
                                  -3.995089457228315E-7,
                                  -1.6037607515301554E-8,
                                  -3.995089457228316E-7,
                                  4.542444291764121E-6));
      properties.get("rightMiddleFingerPitch1").put("offsetFromParentJoint", new Vector3D(0.0022000000000000006, -0.09699999999999998, -0.011899999999999966));
      properties.get("rightMiddleFingerPitch1").put("positionLowerLimit", 0.0);
      properties.get("rightMiddleFingerPitch1").put("positionUpperLimit", 1.658);
      properties.get("rightMiddleFingerPitch1").put("velocityLowerLimit", -1.0);
      properties.get("rightMiddleFingerPitch1").put("velocityUpperLimit", 1.0);
      properties.get("rightMiddleFingerPitch1").put("effortLowerLimit", -10.0);
      properties.get("rightMiddleFingerPitch1").put("effortUpperLimit", 10.0);
      properties.get("rightMiddleFingerPitch1").put("kpPositionLimit", 100.0);
      properties.get("rightMiddleFingerPitch1").put("kdPositionLimit", 20.0);
      properties.get("rightMiddleFingerPitch1").put("kpVelocityLimit", 500.0);
      properties.get("rightMiddleFingerPitch1").put("axis", new Vector3D(0.0, -0.12189598527100426, 0.9925428800685697));
      properties.get("rightMiddleFingerPitch1").put("damping", 1.0);
      properties.get("rightMiddleFingerPitch1").put("stiction", 0.0);
      properties.put("rightMiddleFingerPitch2", new HashMap<>());
      properties.put("rightMiddleFingerPitch2Link", new HashMap<>());
      properties.get("rightMiddleFingerPitch2Link").put("mass", 0.011);
      properties.get("rightMiddleFingerPitch2Link").put("centerOfMass", new Vector3D(0.0, -0.012903057265986551, -0.0015846492326920985));
      properties.get("rightMiddleFingerPitch2Link")
                .put("inertia",
                     new Matrix3D(1.97E-6,
                                  3.508731450793507E-8,
                                  -6.773504308793901E-9,
                                  3.508731450793507E-8,
                                  5.98401161832773E-7,
                                  -2.835378861044921E-7,
                                  -6.773504308793901E-9,
                                  -2.835378861044921E-7,
                                  2.2275988381672266E-6));
      properties.get("rightMiddleFingerPitch2").put("offsetFromParentJoint", new Vector3D(0.0, -0.037820000000000006, -0.004644000000000044));
      properties.get("rightMiddleFingerPitch2").put("positionLowerLimit", 0.0);
      properties.get("rightMiddleFingerPitch2").put("positionUpperLimit", 1.92);
      properties.get("rightMiddleFingerPitch2").put("velocityLowerLimit", -1.0);
      properties.get("rightMiddleFingerPitch2").put("velocityUpperLimit", 1.0);
      properties.get("rightMiddleFingerPitch2").put("effortLowerLimit", -10.0);
      properties.get("rightMiddleFingerPitch2").put("effortUpperLimit", 10.0);
      properties.get("rightMiddleFingerPitch2").put("kpPositionLimit", 100.0);
      properties.get("rightMiddleFingerPitch2").put("kdPositionLimit", 20.0);
      properties.get("rightMiddleFingerPitch2").put("kpVelocityLimit", 500.0);
      properties.get("rightMiddleFingerPitch2").put("axis", new Vector3D(0.0, -0.12189598527100426, 0.9925428800685697));
      properties.get("rightMiddleFingerPitch2").put("damping", 1.0);
      properties.get("rightMiddleFingerPitch2").put("stiction", 0.0);
      properties.put("rightMiddleFingerPitch3", new HashMap<>());
      properties.put("rightMiddleFingerPitch3Link", new HashMap<>());
      properties.get("rightMiddleFingerPitch3Link").put("mass", 0.006);
      properties.get("rightMiddleFingerPitch3Link").put("centerOfMass", new Vector3D(0.0, -0.009925428666143501, -0.001218960948224691));
      properties.get("rightMiddleFingerPitch3Link")
                .put("inertia",
                     new Matrix3D(3.96E-7,
                                  -8.810989704706681E-9,
                                  -2.089607720016572E-9,
                                  -8.810989704706681E-9,
                                  1.5825320090412047E-7,
                                  -1.7766322657938948E-9,
                                  -2.089607720016572E-9,
                                  -1.7766322657938948E-9,
                                  3.0474679909587953E-7));
      properties.get("rightMiddleFingerPitch3").put("offsetFromParentJoint", new Vector3D(0.0, -0.022730000000000167, -0.002791999999999949));
      properties.get("rightMiddleFingerPitch3").put("positionLowerLimit", 0.0);
      properties.get("rightMiddleFingerPitch3").put("positionUpperLimit", 1.57);
      properties.get("rightMiddleFingerPitch3").put("velocityLowerLimit", -1.0);
      properties.get("rightMiddleFingerPitch3").put("velocityUpperLimit", 1.0);
      properties.get("rightMiddleFingerPitch3").put("effortLowerLimit", -10.0);
      properties.get("rightMiddleFingerPitch3").put("effortUpperLimit", 10.0);
      properties.get("rightMiddleFingerPitch3").put("kpPositionLimit", 100.0);
      properties.get("rightMiddleFingerPitch3").put("kdPositionLimit", 20.0);
      properties.get("rightMiddleFingerPitch3").put("kpVelocityLimit", 500.0);
      properties.get("rightMiddleFingerPitch3").put("axis", new Vector3D(0.0, -0.12189598527100426, 0.9925428800685697));
      properties.get("rightMiddleFingerPitch3").put("damping", 1.0);
      properties.get("rightMiddleFingerPitch3").put("stiction", 0.0);
      properties.put("rightPinkyPitch1", new HashMap<>());
      properties.put("rightPinkyPitch1Link", new HashMap<>());
      properties.get("rightPinkyPitch1Link").put("mass", 0.02);
      properties.get("rightPinkyPitch1Link").put("centerOfMass", new Vector3D(0.0, -0.019850857332287003, -0.002437921896449382));
      properties.get("rightPinkyPitch1Link")
                .put("inertia",
                     new Matrix3D(5.148E-6,
                                  -4.034594335475554E-8,
                                  -1.6037607515301554E-8,
                                  -4.034594335475554E-8,
                                  1.37155570823588E-6,
                                  -3.995089457228315E-7,
                                  -1.6037607515301554E-8,
                                  -3.995089457228316E-7,
                                  4.542444291764121E-6));
      properties.get("rightPinkyPitch1").put("offsetFromParentJoint", new Vector3D(0.0022000000000000006, -0.08379999999999999, -0.04099999999999998));
      properties.get("rightPinkyPitch1").put("positionLowerLimit", 0.0);
      properties.get("rightPinkyPitch1").put("positionUpperLimit", 1.57);
      properties.get("rightPinkyPitch1").put("velocityLowerLimit", -1.0);
      properties.get("rightPinkyPitch1").put("velocityUpperLimit", 1.0);
      properties.get("rightPinkyPitch1").put("effortLowerLimit", -10.0);
      properties.get("rightPinkyPitch1").put("effortUpperLimit", 10.0);
      properties.get("rightPinkyPitch1").put("kpPositionLimit", 100.0);
      properties.get("rightPinkyPitch1").put("kdPositionLimit", 20.0);
      properties.get("rightPinkyPitch1").put("kpVelocityLimit", 500.0);
      properties.get("rightPinkyPitch1").put("axis", new Vector3D(0.0, -0.12189598527100426, 0.9925428800685697));
      properties.get("rightPinkyPitch1").put("damping", 1.0);
      properties.get("rightPinkyPitch1").put("stiction", 0.0);
      properties.put("rightPinkyPitch2", new HashMap<>());
      properties.put("rightPinkyPitch2Link", new HashMap<>());
      properties.get("rightPinkyPitch2Link").put("mass", 0.011);
      properties.get("rightPinkyPitch2Link").put("centerOfMass", new Vector3D(0.0, -0.012903057265986551, -0.0015846492326920985));
      properties.get("rightPinkyPitch2Link")
                .put("inertia",
                     new Matrix3D(1.97E-6,
                                  3.508731450793507E-8,
                                  -6.773504308793901E-9,
                                  3.508731450793507E-8,
                                  5.98401161832773E-7,
                                  -2.835378861044921E-7,
                                  -6.773504308793901E-9,
                                  -2.835378861044921E-7,
                                  2.2275988381672266E-6));
      properties.get("rightPinkyPitch2").put("offsetFromParentJoint", new Vector3D(0.0, -0.037816000000000016, -0.0046439999999999025));
      properties.get("rightPinkyPitch2").put("positionLowerLimit", 0.0);
      properties.get("rightPinkyPitch2").put("positionUpperLimit", 1.658);
      properties.get("rightPinkyPitch2").put("velocityLowerLimit", -1.0);
      properties.get("rightPinkyPitch2").put("velocityUpperLimit", 1.0);
      properties.get("rightPinkyPitch2").put("effortLowerLimit", -10.0);
      properties.get("rightPinkyPitch2").put("effortUpperLimit", 10.0);
      properties.get("rightPinkyPitch2").put("kpPositionLimit", 100.0);
      properties.get("rightPinkyPitch2").put("kdPositionLimit", 20.0);
      properties.get("rightPinkyPitch2").put("kpVelocityLimit", 500.0);
      properties.get("rightPinkyPitch2").put("axis", new Vector3D(0.0, -0.12189598527100426, 0.9925428800685697));
      properties.get("rightPinkyPitch2").put("damping", 1.0);
      properties.get("rightPinkyPitch2").put("stiction", 0.0);
      properties.put("rightPinkyPitch3", new HashMap<>());
      properties.put("rightPinkyPitch3Link", new HashMap<>());
      properties.get("rightPinkyPitch3Link").put("mass", 0.006);
      properties.get("rightPinkyPitch3Link").put("centerOfMass", new Vector3D(0.0, -0.009925428666143501, -0.001218960948224691));
      properties.get("rightPinkyPitch3Link")
                .put("inertia",
                     new Matrix3D(3.96E-7,
                                  -8.810989704706681E-9,
                                  -2.089607720016572E-9,
                                  -8.810989704706681E-9,
                                  1.5825320090412047E-7,
                                  -1.7766322657938948E-9,
                                  -2.089607720016572E-9,
                                  -1.7766322657938948E-9,
                                  3.0474679909587953E-7));
      properties.get("rightPinkyPitch3").put("offsetFromParentJoint", new Vector3D(0.0, -0.022734000000000056, -0.0027920000000000765));
      properties.get("rightPinkyPitch3").put("positionLowerLimit", 0.0);
      properties.get("rightPinkyPitch3").put("positionUpperLimit", 1.92);
      properties.get("rightPinkyPitch3").put("velocityLowerLimit", -1.0);
      properties.get("rightPinkyPitch3").put("velocityUpperLimit", 1.0);
      properties.get("rightPinkyPitch3").put("effortLowerLimit", -10.0);
      properties.get("rightPinkyPitch3").put("effortUpperLimit", 10.0);
      properties.get("rightPinkyPitch3").put("kpPositionLimit", 100.0);
      properties.get("rightPinkyPitch3").put("kdPositionLimit", 20.0);
      properties.get("rightPinkyPitch3").put("kpVelocityLimit", 500.0);
      properties.get("rightPinkyPitch3").put("axis", new Vector3D(0.0, -0.12189598527100426, 0.9925428800685697));
      properties.get("rightPinkyPitch3").put("damping", 1.0);
      properties.get("rightPinkyPitch3").put("stiction", 0.0);
      properties.put("rightThumbRoll", new HashMap<>());
      properties.put("rightThumbRollLink", new HashMap<>());
      properties.get("rightThumbRollLink").put("mass", 0.017);
      properties.get("rightThumbRollLink").put("centerOfMass", new Vector3D(0.0, -0.003420522332544199, 0.009396809403865038));
      properties.get("rightThumbRollLink")
                .put("inertia",
                     new Matrix3D(2.77788E-6,
                                  3.800365322933979E-8,
                                  -8.7821261445544E-8,
                                  3.800365322933979E-8,
                                  3.2663699259248467E-6,
                                  7.322727571414368E-7,
                                  -8.7821261445544E-8,
                                  7.322727571414366E-7,
                                  1.6352200740751536E-6));
      properties.get("rightThumbRoll").put("offsetFromParentJoint", new Vector3D(0.0049, -0.03510000000000002, 0.022800000000000042));
      properties.get("rightThumbRoll").put("positionLowerLimit", 0.0);
      properties.get("rightThumbRoll").put("positionUpperLimit", 2.356);
      properties.get("rightThumbRoll").put("velocityLowerLimit", -1.0);
      properties.get("rightThumbRoll").put("velocityUpperLimit", 1.0);
      properties.get("rightThumbRoll").put("effortLowerLimit", -10.0);
      properties.get("rightThumbRoll").put("effortUpperLimit", 10.0);
      properties.get("rightThumbRoll").put("kpPositionLimit", 100.0);
      properties.get("rightThumbRoll").put("kdPositionLimit", 20.0);
      properties.get("rightThumbRoll").put("kpVelocityLimit", 500.0);
      properties.get("rightThumbRoll").put("axis", new Vector3D(0.0, 0.939681022333869, 0.34205200812972125));
      properties.get("rightThumbRoll").put("damping", 1.0);
      properties.get("rightThumbRoll").put("stiction", 0.0);
      properties.put("rightThumbPitch1", new HashMap<>());
      properties.put("rightThumbPitch1Link", new HashMap<>());
      properties.get("rightThumbPitch1Link").put("mass", 0.02);
      properties.get("rightThumbPitch1Link").put("centerOfMass", new Vector3D(0.0, -0.005, 0.02));
      properties.get("rightThumbPitch1Link").put("inertia", new Matrix3D(4.239E-6, 0.0, -0.0, 0.0, 4.582E-6, 0.0, -0.0, 0.0, 1.47E-6));
      properties.get("rightThumbPitch1").put("offsetFromParentJoint", new Vector3D(0.0, -0.007832999999999965, 0.021518999999999993));
      properties.get("rightThumbPitch1").put("positionLowerLimit", 0.0);
      properties.get("rightThumbPitch1").put("positionUpperLimit", 1.658);
      properties.get("rightThumbPitch1").put("velocityLowerLimit", -1.0);
      properties.get("rightThumbPitch1").put("velocityUpperLimit", 1.0);
      properties.get("rightThumbPitch1").put("effortLowerLimit", -10.0);
      properties.get("rightThumbPitch1").put("effortUpperLimit", 10.0);
      properties.get("rightThumbPitch1").put("kpPositionLimit", 100.0);
      properties.get("rightThumbPitch1").put("kdPositionLimit", 20.0);
      properties.get("rightThumbPitch1").put("kpVelocityLimit", 500.0);
      properties.get("rightThumbPitch1").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("rightThumbPitch1").put("damping", 1.0);
      properties.get("rightThumbPitch1").put("stiction", 0.0);
      properties.put("rightThumbPitch2", new HashMap<>());
      properties.put("rightThumbPitch2Link", new HashMap<>());
      properties.get("rightThumbPitch2Link").put("mass", 0.013);
      properties.get("rightThumbPitch2Link").put("centerOfMass", new Vector3D(0.0, -0.004, 0.017));
      properties.get("rightThumbPitch2Link").put("inertia", new Matrix3D(1.266E-6, 0.0, -0.0, 0.0, 1.503E-6, -0.0, -0.0, -0.0, 6.99E-7));
      properties.get("rightThumbPitch2").put("offsetFromParentJoint", new Vector3D(0.0, -0.00660000000000005, 0.03750000000000003));
      properties.get("rightThumbPitch2").put("positionLowerLimit", 0.0);
      properties.get("rightThumbPitch2").put("positionUpperLimit", 1.92);
      properties.get("rightThumbPitch2").put("velocityLowerLimit", -1.0);
      properties.get("rightThumbPitch2").put("velocityUpperLimit", 1.0);
      properties.get("rightThumbPitch2").put("effortLowerLimit", -10.0);
      properties.get("rightThumbPitch2").put("effortUpperLimit", 10.0);
      properties.get("rightThumbPitch2").put("kpPositionLimit", 100.0);
      properties.get("rightThumbPitch2").put("kdPositionLimit", 20.0);
      properties.get("rightThumbPitch2").put("kpVelocityLimit", 500.0);
      properties.get("rightThumbPitch2").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("rightThumbPitch2").put("damping", 1.0);
      properties.get("rightThumbPitch2").put("stiction", 0.0);
      properties.put("rightThumbPitch3", new HashMap<>());
      properties.put("rightThumbPitch3Link", new HashMap<>());
      properties.get("rightThumbPitch3Link").put("mass", 0.006);
      properties.get("rightThumbPitch3Link").put("centerOfMass", new Vector3D(0.0, -0.004, 0.01));
      properties.get("rightThumbPitch3Link").put("inertia", new Matrix3D(3.22E-7, 0.0, 0.0, 0.0, 4.06E-7, 0.0, 0.0, 0.0, 2.1E-7));
      properties.get("rightThumbPitch3").put("offsetFromParentJoint", new Vector3D(0.0, -0.004899999999999904, 0.02749999999999997));
      properties.get("rightThumbPitch3").put("positionLowerLimit", 0.0);
      properties.get("rightThumbPitch3").put("positionUpperLimit", 1.57);
      properties.get("rightThumbPitch3").put("velocityLowerLimit", -1.0);
      properties.get("rightThumbPitch3").put("velocityUpperLimit", 1.0);
      properties.get("rightThumbPitch3").put("effortLowerLimit", -10.0);
      properties.get("rightThumbPitch3").put("effortUpperLimit", 10.0);
      properties.get("rightThumbPitch3").put("kpPositionLimit", 100.0);
      properties.get("rightThumbPitch3").put("kdPositionLimit", 20.0);
      properties.get("rightThumbPitch3").put("kpVelocityLimit", 500.0);
      properties.get("rightThumbPitch3").put("axis", new Vector3D(1.0, 0.0, 0.0));
      properties.get("rightThumbPitch3").put("damping", 1.0);
      properties.get("rightThumbPitch3").put("stiction", 0.0);

      return properties;
   }

   public static Map> valkyrieSensorProperties()
   {
      Map> sensorProperties = new HashMap<>();
      sensorProperties.put("pelvisMiddleImu", new HashMap<>());
      sensorProperties.get("pelvisMiddleImu")
                      .put("transformToJoint",
                           new RigidBodyTransform(1.0,
                                                  0.0,
                                                  0.0,
                                                  0.0,
                                                  0.0,
                                                  -0.9999999999964793,
                                                  -2.65358979335273E-6,
                                                  0.0,
                                                  0.0,
                                                  2.65358979335273E-6,
                                                  -0.9999999999964793,
                                                  -0.108196));
      sensorProperties.get("pelvisMiddleImu").put("accelerationNoiseMean", 0.0);
      sensorProperties.get("pelvisMiddleImu").put("accelerationNoiseStandardDeviation", 0.017);
      sensorProperties.get("pelvisMiddleImu").put("accelerationBiasMean", 0.1);
      sensorProperties.get("pelvisMiddleImu").put("accelerationBiasStandardDeviation", 0.001);
      sensorProperties.get("pelvisMiddleImu").put("angularVelocityNoiseMean", 7.5E-6);
      sensorProperties.get("pelvisMiddleImu").put("angularVelocityNoiseStandardDeviation", 8.0E-7);
      sensorProperties.get("pelvisMiddleImu").put("angularVelocityBiasMean", 0.0);
      sensorProperties.get("pelvisMiddleImu").put("angularVelocityBiasStandardDeviation", 0.0);
      sensorProperties.put("pelvisRearImu", new HashMap<>());
      sensorProperties.get("pelvisRearImu")
                      .put("transformToJoint",
                           new RigidBodyTransform(-0.8886208494403026,
                                                  -1.4365406117855013E-6,
                                                  0.4586425470210231,
                                                  -0.0758449,
                                                  -2.358035216243522E-6,
                                                  0.999999999996188,
                                                  -1.4365406117855013E-6,
                                                  0.0,
                                                  -0.4586425470172111,
                                                  -2.358035216243522E-6,
                                                  -0.8886208494403026,
                                                  -0.111056));
      sensorProperties.get("pelvisRearImu").put("accelerationNoiseMean", 0.0);
      sensorProperties.get("pelvisRearImu").put("accelerationNoiseStandardDeviation", 0.017);
      sensorProperties.get("pelvisRearImu").put("accelerationBiasMean", 0.1);
      sensorProperties.get("pelvisRearImu").put("accelerationBiasStandardDeviation", 0.001);
      sensorProperties.get("pelvisRearImu").put("angularVelocityNoiseMean", 7.5E-6);
      sensorProperties.get("pelvisRearImu").put("angularVelocityNoiseStandardDeviation", 8.0E-7);
      sensorProperties.get("pelvisRearImu").put("angularVelocityBiasMean", 0.0);
      sensorProperties.get("pelvisRearImu").put("angularVelocityBiasStandardDeviation", 0.0);
      sensorProperties.put("leftHazardCamera___default__", new HashMap<>());
      sensorProperties.get("leftHazardCamera___default__")
                      .put("transformToJoint",
                           new RigidBodyTransform(7.963267107332633E-4,
                                                  -7.963264582434141E-4,
                                                  0.9999993658637698,
                                                  0.0345,
                                                  0.9999996829318346,
                                                  6.341362301717473E-7,
                                                  -7.963264582434141E-4,
                                                  0.0406,
                                                  5.55112E-17,
                                                  0.9999996829318346,
                                                  7.963267107332633E-4,
                                                  0.1135));
      sensorProperties.get("leftHazardCamera___default__").put("fieldOfView", 1.378);
      sensorProperties.get("leftHazardCamera___default__").put("clipNear", 0.1);
      sensorProperties.get("leftHazardCamera___default__").put("clipFar", 100.0);
      sensorProperties.get("leftHazardCamera___default__").put("imageWidth", 1280);
      sensorProperties.get("leftHazardCamera___default__").put("imageHeight", 1024);
      sensorProperties.put("rightHazardCamera___default__", new HashMap<>());
      sensorProperties.get("rightHazardCamera___default__")
                      .put("transformToJoint",
                           new RigidBodyTransform(7.963267107332633E-4,
                                                  -7.963264582434141E-4,
                                                  0.9999993658637698,
                                                  0.0345,
                                                  0.9999996829318346,
                                                  6.341362301717473E-7,
                                                  -7.963264582434141E-4,
                                                  -0.0406,
                                                  5.55112E-17,
                                                  0.9999996829318346,
                                                  7.963267107332633E-4,
                                                  0.1135));
      sensorProperties.get("rightHazardCamera___default__").put("fieldOfView", 1.378);
      sensorProperties.get("rightHazardCamera___default__").put("clipNear", 0.1);
      sensorProperties.get("rightHazardCamera___default__").put("clipFar", 100.0);
      sensorProperties.get("rightHazardCamera___default__").put("imageWidth", 1280);
      sensorProperties.get("rightHazardCamera___default__").put("imageHeight", 1024);
      sensorProperties.put("leftTorsoImu", new HashMap<>());
      sensorProperties.get("leftTorsoImu")
                      .put("transformToJoint",
                           new RigidBodyTransform(1.0,
                                                  -0.0,
                                                  0.0,
                                                  -0.0627634,
                                                  0.0,
                                                  9.632679474766714E-5,
                                                  0.9999999953605743,
                                                  0.134239,
                                                  -0.0,
                                                  -0.9999999953605743,
                                                  9.632679474766714E-5,
                                                  0.363068));
      sensorProperties.get("leftTorsoImu").put("accelerationNoiseMean", 0.0);
      sensorProperties.get("leftTorsoImu").put("accelerationNoiseStandardDeviation", 0.017);
      sensorProperties.get("leftTorsoImu").put("accelerationBiasMean", 0.1);
      sensorProperties.get("leftTorsoImu").put("accelerationBiasStandardDeviation", 0.001);
      sensorProperties.get("leftTorsoImu").put("angularVelocityNoiseMean", 7.5E-6);
      sensorProperties.get("leftTorsoImu").put("angularVelocityNoiseStandardDeviation", 8.0E-7);
      sensorProperties.get("leftTorsoImu").put("angularVelocityBiasMean", 0.0);
      sensorProperties.get("leftTorsoImu").put("angularVelocityBiasStandardDeviation", 0.0);
      sensorProperties.put("stereo_camera_left", new HashMap<>());
      sensorProperties.get("stereo_camera_left")
                      .put("transformToJoint",
                           new RigidBodyTransform(0.991444821419641,
                                                  -3.46363776756234E-7,
                                                  -0.13052649570127964,
                                                  0.183847,
                                                  5.293958426245172E-23,
                                                  -0.9999999999964793,
                                                  2.65358979335273E-6,
                                                  -0.035,
                                                  -0.1305264957017392,
                                                  -2.6308878587915793E-6,
                                                  -0.9914448214161503,
                                                  0.0773366));
      sensorProperties.get("stereo_camera_left").put("fieldOfView", 1.39626);
      sensorProperties.get("stereo_camera_left").put("clipNear", 0.02);
      sensorProperties.get("stereo_camera_left").put("clipFar", 300.0);
      sensorProperties.get("stereo_camera_left").put("imageWidth", 1024);
      sensorProperties.get("stereo_camera_left").put("imageHeight", 544);
      sensorProperties.put("stereo_camera_right", new HashMap<>());
      sensorProperties.get("stereo_camera_right")
                      .put("transformToJoint",
                           new RigidBodyTransform(0.991444821419641,
                                                  -3.46363776756234E-7,
                                                  -0.13052649570127964,
                                                  0.1838470242454644,
                                                  5.293958426245172E-23,
                                                  -0.9999999999964793,
                                                  2.65358979335273E-6,
                                                  0.03499999999975355,
                                                  -0.1305264957017392,
                                                  -2.6308878587915793E-6,
                                                  -0.9914448214161503,
                                                  0.07733678416215012));
      sensorProperties.get("stereo_camera_right").put("fieldOfView", 1.39626);
      sensorProperties.get("stereo_camera_right").put("clipNear", 0.02);
      sensorProperties.get("stereo_camera_right").put("clipFar", 300.0);
      sensorProperties.get("stereo_camera_right").put("imageWidth", 1024);
      sensorProperties.get("stereo_camera_right").put("imageHeight", 544);
      sensorProperties.put("head_imu_sensor", new HashMap<>());
      sensorProperties.get("head_imu_sensor")
                      .put("transformToJoint",
                           new RigidBodyTransform(0.991444821419641,
                                                  -3.46363776756234E-7,
                                                  -0.13052649570127964,
                                                  0.136492,
                                                  5.293958426245172E-23,
                                                  -0.9999999999964793,
                                                  2.65358979335273E-6,
                                                  -0.035,
                                                  -0.1305264957017392,
                                                  -2.6308878587915793E-6,
                                                  -0.9914448214161503,
                                                  0.0815537));
      sensorProperties.get("head_imu_sensor").put("accelerationNoiseMean", 0.0);
      sensorProperties.get("head_imu_sensor").put("accelerationNoiseStandardDeviation", 0.017);
      sensorProperties.get("head_imu_sensor").put("accelerationBiasMean", 0.1);
      sensorProperties.get("head_imu_sensor").put("accelerationBiasStandardDeviation", 0.001);
      sensorProperties.get("head_imu_sensor").put("angularVelocityNoiseMean", 7.5E-6);
      sensorProperties.get("head_imu_sensor").put("angularVelocityNoiseStandardDeviation", 8.0E-7);
      sensorProperties.get("head_imu_sensor").put("angularVelocityBiasMean", 0.0);
      sensorProperties.get("head_imu_sensor").put("angularVelocityBiasStandardDeviation", 0.0);
      sensorProperties.put("head_hokuyo_sensor", new HashMap<>());
      sensorProperties.get("head_hokuyo_sensor")
                      .put("transformToJoint",
                           new RigidBodyTransform(0.991444821419641,
                                                  -3.46363776756234E-7,
                                                  -0.13052649570127964,
                                                  0.027785447207070033,
                                                  5.293958426245172E-23,
                                                  -0.9999999999964793,
                                                  2.65358979335273E-6,
                                                  3.9803846900290945E-8,
                                                  -0.1305264957017392,
                                                  -2.6308878587915793E-6,
                                                  -0.9914448214161503,
                                                  -0.01878746719229443));
      sensorProperties.get("head_hokuyo_sensor").put("sweepYawMin", -2.35619);
      sensorProperties.get("head_hokuyo_sensor").put("sweepYawMax", 2.35619);
      sensorProperties.get("head_hokuyo_sensor").put("heightPitchMin", 0.0);
      sensorProperties.get("head_hokuyo_sensor").put("heightPitchMax", 0.0);
      sensorProperties.get("head_hokuyo_sensor").put("minRange", 0.1);
      sensorProperties.get("head_hokuyo_sensor").put("maxRange", 30.0);
      sensorProperties.get("head_hokuyo_sensor").put("pointsPerSweep", 1080);
      sensorProperties.get("head_hokuyo_sensor").put("scanHeight", 1);
      return sensorProperties;
   }
}




© 2015 - 2025 Weber Informatics LLC | Privacy Policy