us.ihmc.simulationconstructionset.robotdefinition.JointDefinitionFixedFrame Maven / Gradle / Ivy

Go to download
Show more of this group Show more artifacts with this name
Show all versions of simulation-construction-set
Simulation Construction Set
There is a newer version: 0.25.2
package us.ihmc.simulationconstructionset.robotdefinition;

import java.util.ArrayList;

import us.ihmc.euclid.tuple3D.Vector3D;
import us.ihmc.robotics.robotDescription.Plane;

public class JointDefinitionFixedFrame implements Comparable
{
   public enum JointType
   {
      PIN_JOINT, SLIDER_JOINT, FLOATING_JOINT, FLOATING_PLANAR_JOINT
   }

   private String jointName;

   private JointDefinitionFixedFrame parentJoint = null;

   // temporary use for creating jointdefinitions from files
   private String parentName = null;

   public String getParentName()
   {
      return parentName;
   }

   public void setParentName(String parentName)
   {
      this.parentName = parentName;
   }

   private JointType type;
   private boolean rootJoint = false;
   private LinkDefinitionFixedFrame link;

   private Vector3D offset;
   private Vector3D jointAxis;

   private ArrayList childrenJoints = new ArrayList<>();

   // only used for planar joints.
   private Plane planarType = Plane.YZ;

   private ArrayList groundContactDefinitionsFixedFrame = new ArrayList<>();
   private ArrayList externalForcePointDefinitionsFixedFrame = new ArrayList<>();

   public Plane getPlanarType()
   {
      return planarType;
   }

   public void setRootJoint(boolean rootJoint)
   {
      this.rootJoint = rootJoint;
   }

   public boolean isRootJoint()
   {
      return rootJoint;
   }

   public ArrayList getGroundContactDefinitionsFixedFrame()
   {
      return groundContactDefinitionsFixedFrame;
   }

   public void addGroundContactDefinitionFixedFrame(GroundContactDefinitionFixedFrame groundContactDefinitionFixedFrame)
   {
      groundContactDefinitionsFixedFrame.add(groundContactDefinitionFixedFrame);
   }

   public ArrayList getExternalForcePointDefinitionsFixedFrame()
   {
      return externalForcePointDefinitionsFixedFrame;
   }

   public void addExternalForcePointDefinition(ExternalForcePointDefinitionFixedFrame externalForcePointDefinitionFixedFrame)
   {
      externalForcePointDefinitionsFixedFrame.add(externalForcePointDefinitionFixedFrame);
   }

   public void setPlanarType(Plane type)
   {
      planarType = type;
   }

   public int getNumberOfChildJoints()
   {
      int numberOfJoints = childrenJoints.size();
      for (JointDefinitionFixedFrame joint : childrenJoints)
      {
         numberOfJoints += joint.getNumberOfChildJoints();
      }

      return numberOfJoints;
   }

   public String getJointName()
   {
      return jointName;
   }

   public void setJointName(String jointName)
   {
      this.jointName = jointName;
   }

   public JointDefinitionFixedFrame getParentJoint()
   {
      return parentJoint;
   }

   public void setParentJoint(JointDefinitionFixedFrame parentJoint)
   {
      this.parentJoint = parentJoint;
   }

   public JointType getType()
   {
      return type;
   }

   public void setType(JointType type)
   {
      this.type = type;
   }

   public LinkDefinitionFixedFrame getLinkDefinition()
   {
      return link;
   }

   public void setLinkDefinition(LinkDefinitionFixedFrame link)
   {
      this.link = link;
   }

   public Vector3D getOffset()
   {
      return offset;
   }

   public void setOffset(Vector3D offset)
   {
      this.offset = offset;
   }

   public Vector3D getJointAxis()
   {
      return jointAxis;
   }

   public void setJointAxis(Vector3D jointAxis)
   {
      this.jointAxis = jointAxis;
   }

   public ArrayList getChildrenJoints()
   {
      return childrenJoints;
   }

   public void addChildJoint(JointDefinitionFixedFrame jointDef)
   {
      childrenJoints.add(jointDef);
   }

   @Override
   public String toString()
   {
      String returnString = "";

      //    if (parentJoint == null)
      //       returnString += "Found Root Joint.\n";

      returnString += "\n";
      returnString += "\t" + jointName + "\n";

      returnString += "\t";
      if (parentJoint != null)
         returnString += parentJoint.getJointName();
      else
         returnString += "null";
      returnString += "\n";

      returnString += "\t" + rootJoint + "\n";

      //    returnString += "Joint name = " + jointName + "\n";

      returnString += "\t" + offset + "\n";

      //    returnString += "Joint offset = " + offset + "\n";

      returnString += "\t" + jointAxis + "\n";

      //    returnString += "Joint axis = " + jointAxis + "\n";

      returnString += "\t" + type + "\n";

      if (type == JointType.PIN_JOINT)
      {
         //       returnString += "Joint is a Pin Joint.\n";
         //       returnString += "Its q variable is named q_" + jointName + "\n";
      }

      else if (type == JointType.SLIDER_JOINT)
      {
         //       returnString += "Joint is a Slider Joint.\n";

         //       returnString += "Its q variable is named q_" + jointName + "\n";
      }

      else if (type == JointType.FLOATING_JOINT)
      {
         //       returnString += "Joint is a Floating Joint.\n";
      }

      else if (type == JointType.FLOATING_PLANAR_JOINT)
      {
         //       returnString += "Joint is a Floating Planar Joint.\n";
      }

      else
      {
         throw new RuntimeException("Only Pin, Slider, Floating, and Floating Planar joints implemented right now");
      }

      //    for (GroundContactDefinitionFixedFrame gcDefinition : groundContactDefinitionsFixedFrame)
      //    {
      //       returnString += gcDefinition.toString();
      //    }

      returnString += getGroundContactPointsString();
      returnString += getExternalForcePointsString();
      returnString += getLinkDefinition();
      returnString += "\n";

      return returnString;
   }

   private String getGroundContactPointsString()
   {
      String returnString = "\n";
      for (GroundContactDefinitionFixedFrame gcDef : groundContactDefinitionsFixedFrame)
      {
         returnString += gcDef.getXMLRepresentation();
      }

      returnString += "\n";

      return returnString;
   }

   private String getExternalForcePointsString()
   {
      String returnString = "\n";
      for (ExternalForcePointDefinitionFixedFrame efpDef : externalForcePointDefinitionsFixedFrame)
      {
         returnString += efpDef.getXMLRepresentation();
      }

      returnString += "\n";

      return returnString;
   }

   @Override
   public int compareTo(JointDefinitionFixedFrame o)
   {
      return o.getJointName().compareTo(getJointName());
   }

}