us.ihmc.simulationconstructionset.OneDegreeOfFreedomJoint 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;

import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly;
import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly;
import us.ihmc.yoVariables.variable.YoDouble;

public abstract class OneDegreeOfFreedomJoint extends Joint
{
   private static final long serialVersionUID = -462519687462777726L;

   /*
    * VRC HACK
    */
   private double kp = 0;
   private double kd = 0;
   private double qDesired = 0;
   private double qdDesired = 0;

   public OneDegreeOfFreedomJoint(String jname, Tuple3DReadOnly offset, Robot rob)
   {
      super(jname, offset, rob, 1);
   }

   public abstract YoDouble getQDDYoVariable();

   public abstract double getQDD();

   public abstract YoDouble getQDYoVariable();

   public abstract double getQD();

   public abstract YoDouble getQYoVariable();

   public abstract double getQ();

   public abstract void setQdd(double qdd);

   public abstract void setQd(double qd);

   public abstract void setQ(double q);

   public abstract void setTau(double tau);

   public abstract YoDouble getTauYoVariable();

   public abstract double getTau();

   public abstract double getDamping();

   public abstract void setDamping(double b_damp);

   public abstract double getTorqueLimit();

   public abstract double getVelocityLimit();

   public abstract double getJointUpperLimit();

   public abstract double getJointLowerLimit();

   public abstract double getJointStiction();

   /*
    * VRC HACK
    */
   public double getKp()
   {
      return kp;
   }

   public double getKd()
   {
      return kd;
   }

   public double getqDesired()
   {
      return qDesired;
   }

   public double getQdDesired()
   {
      return qdDesired;
   }

   public void setKp(double kp)
   {
      this.kp = kp;
   }

   public void setKd(double kd)
   {
      this.kd = kd;
   }

   public void setqDesired(double qDesired)
   {
      this.qDesired = qDesired;
   }

   public void setQdDesired(double qdDesired)
   {
      this.qdDesired = qdDesired;
   }

   public double doPDControl()
   {
      double qError = qDesired - getQYoVariable().getDoubleValue();
      double qdError = qdDesired - getQDYoVariable().getDoubleValue();

      return kp * qError + kd * qdError;
   }
}