Merge pull request #291 from borglab/refactor/kill_JointTyped

Kill JointTyped

examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py

@@ -112,10 +112,10 @@ def time(values, id, t):
         key: [fn(results, j1_id, t) for t in range(t_steps + 1)]
         for (key, fn) in [
             ("t", time),  #
-            ("theta", gtd.JointAngleDouble),
-            ("dtheta", gtd.JointVelDouble),
-            ("ddtheta", gtd.JointAccelDouble),
-            ("tau", gtd.TorqueDouble)
+            ("theta", gtd.JointAngle),
+            ("dtheta", gtd.JointVel),
+            ("ddtheta", gtd.JointAccel),
+            ("tau", gtd.Torque)
         ]
     }
     df = pd.DataFrame(data)

examples/example_jumping_robot/src/jr_simulator.py

@@ -134,7 +134,7 @@ def step_robot_dynamics_by_layer(self, k, values):
             for joint in self.jr.robot.joints():
                 j = joint.id()
                 q_key = gtd.internal.JointAngleKey(j, k).key()
-                graph_q.add(gtd.PriorFactorDouble(q_key, gtd.JointAngleDouble(values, j, k), opt.prior_q_cost_model))
+                graph_q.add(gtd.PriorFactorDouble(q_key, gtd.JointAngle(values, j, k), opt.prior_q_cost_model))
 
         init_values_q = gtd.ExtractValues(init_values, graph_q.keys())
         results_q = self.optimize(graph_q, init_values_q)
@@ -153,7 +153,7 @@ def step_robot_dynamics_by_layer(self, k, values):
             for joint in self.jr.robot.joints():
                 j = joint.id()
                 v_key = gtd.internal.JointVelKey(j, k).key()
-                graph_v.add(gtd.PriorFactorDouble(v_key, gtd.JointVelDouble(values, j, k), opt.prior_qv_cost_model))
+                graph_v.add(gtd.PriorFactorDouble(v_key, gtd.JointVel(values, j, k), opt.prior_qv_cost_model))
 
         init_values_v = gtd.ExtractValues(init_values, graph_v.keys())
         results_v = self.optimize(graph_v, init_values_v)
@@ -230,9 +230,9 @@ def step_robot_dynamics(self, k, values):
             for joint in self.jr.robot.joints():
                 j = joint.id()
                 q_key = gtd.internal.JointAngleKey(j, k).key()
-                graph.add(gtd.PriorFactorDouble(q_key, gtd.JointAngleDouble(values, j, k), opt.prior_q_cost_model))
+                graph.add(gtd.PriorFactorDouble(q_key, gtd.JointAngle(values, j, k), opt.prior_q_cost_model))
                 v_key = gtd.internal.JointVelKey(j, k).key()
-                graph.add(gtd.PriorFactorDouble(v_key, gtd.JointVelDouble(values, j, k), opt.prior_v_cost_model))
+                graph.add(gtd.PriorFactorDouble(v_key, gtd.JointVel(values, j, k), opt.prior_v_cost_model))
 
         # construct initial values
         if k==0:
@@ -347,7 +347,7 @@ def simulate_with_torque_seq(self, num_steps, dt, torques_seq):
                 self.step_integration(k, dt, values, False)
             for joint in self.jr.robot.joints():
                 j = joint.id()
-                gtd.InsertTorqueDouble(values, j, k, torques_seq[k][j])
+                gtd.InsertTorque(values, j, k, torques_seq[k][j])
             self.step_robot_dynamics_by_layer(k, values)
             phase = self.step_phase_change(k, phase, values)
             step_phases.append(phase)

examples/example_jumping_robot/src/jr_values.py

@@ -232,8 +232,8 @@ def init_values_from_prev_robot(robot, k, values):
             v_key = gtd.internal.JointVelKey(j, k).key()
             JRValues.copy_value(values, init_values, q_key)
             JRValues.copy_value(values, init_values, v_key)
-            gtd.InsertJointAccelDouble(
-                init_values, j, k, gtd.JointAccelDouble(values, j, k-1))
+            gtd.InsertJointAccel(
+                init_values, j, k, gtd.JointAccel(values, j, k-1))
             i1 = joint.parent().id()
             i2 = joint.child().id()
             gtd.InsertWrench(init_values, i1, j, k,
@@ -300,17 +300,17 @@ def init_values_from_fk_robot(jr, k, values):
                 v = values.atDouble(v_key)
             init_values.insert(q_key, q)
             init_values.insert(v_key, v)
-            gtd.InsertJointAccelDouble(init_values, j, k, 0.0)
+            gtd.InsertJointAccel(init_values, j, k, 0.0)
             i1 = joint.parent().id()
             i2 = joint.child().id()
             gtd.InsertWrench(init_values, i1, j, k, np.zeros(6))
             gtd.InsertWrench(init_values, i2, j, k, np.zeros(6))
             torque_key = gtd.internal.TorqueKey(j, k).key()
             if values.exists(torque_key):
-                gtd.InsertTorqueDouble(
+                gtd.InsertTorque(
                     init_values, j, k, values.atDouble(torque_key))
             else:
-                gtd.InsertTorqueDouble(init_values, j, k, 0.0)
+                gtd.InsertTorque(init_values, j, k, 0.0)
 
         return init_values
 
@@ -319,13 +319,13 @@ def integrate_joints(jr, values, k, dt):
         """ Integrate joint angle and velocity and add to values. """
         for joint in jr.robot.joints():
             j = joint.id()
-            q_prev = gtd.JointAngleDouble(values, j, k-1)
-            v_prev = gtd.JointVelDouble(values, j, k-1)
-            a_prev = gtd.JointAccelDouble(values, j, k-1)
+            q_prev = gtd.JointAngle(values, j, k-1)
+            v_prev = gtd.JointVel(values, j, k-1)
+            a_prev = gtd.JointAccel(values, j, k-1)
             v_curr = v_prev + a_prev * dt
             q_curr = q_prev + v_prev * dt + 0.5 * a_prev * dt * dt
-            gtd.InsertJointAngleDouble(values, j, k, q_curr)
-            gtd.InsertJointVelDouble(values, j, k, v_curr)
+            gtd.InsertJointAngle(values, j, k, q_curr)
+            gtd.InsertJointVel(values, j, k, v_curr)
 
     @staticmethod
     def integrate_torso(jr, values, k, dt):

examples/example_jumping_robot/src/jr_visualizer.py

@@ -119,9 +119,9 @@ def make_plot(values, jr, num_steps):
     for k in range(num_steps):
         for name in joint_names:
             j = jr.robot.joint(name).id()
-            qs_dict[name].append(gtd.JointAngleDouble(values, j, k))
-            vs_dict[name].append(gtd.JointVelDouble(values, j, k))
-            torques_dict[name].append(gtd.TorqueDouble(values, j, k))
+            qs_dict[name].append(gtd.JointAngle(values, j, k))
+            vs_dict[name].append(gtd.JointVel(values, j, k))
+            torques_dict[name].append(gtd.Torque(values, j, k))
             pressures_dict[name].append(values.atDouble(Actuator.PressureKey(j, k)))
             masses_dict[name].append(values.atDouble(Actuator.MassKey(j, k)))
             mdots_dict[name].append(values.atDouble(Actuator.MassRateActualKey(j, k)))

examples/example_jumping_robot/tests/test_jr_simulator.py

@@ -74,9 +74,9 @@ def test_robot_forward_dynamics(self):
         k = 0
         for joint in self.robot().joints():
             j = joint.id()
-            gtd.InsertTorqueDouble(values, j, k, torques[j])
-            gtd.InsertJointAngleDouble(values, j, k, qs[j])
-            gtd.InsertJointVelDouble(values, j, k, vs[j])
+            gtd.InsertTorque(values, j, k, torques[j])
+            gtd.InsertJointAngle(values, j, k, qs[j])
+            gtd.InsertJointVel(values, j, k, vs[j])
         torso_pose = gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0, 0, 0.55))
         torso_i = self.robot().link("torso").id()
         gtd.InsertPose(values, torso_i, k, torso_pose)
@@ -114,7 +114,7 @@ def test_actuation_forward_dynamics(self):
         torques = []
         for actuator in self.jr_simulator.jr.actuators:
             j = actuator.j
-            torque = gtd.TorqueDouble(values, j, k)
+            torque = gtd.Torque(values, j, k)
             torques.append(torque)
             self.assertAlmostEqual(torque, 0, places=7)
         # TODO(yetong): check torques, pressures, etc

examples/example_jumping_robot/tests/test_jumping_robot.py

@@ -48,8 +48,8 @@ def test_forward_kinematics(self):
         qs = [-theta, 2 * theta, -theta, -theta, 2 * theta, -theta]
         for joint in self.jr.robot.joints():
             j = joint.id()
-            gtd.InsertJointAngleDouble(values, j, k, qs[j])
-            gtd.InsertJointVelDouble(values, j, k, 0.)
+            gtd.InsertJointAngle(values, j, k, qs[j])
+            gtd.InsertJointVel(values, j, k, 0.)
 
         fk_results = self.jr.robot.forwardKinematics(values, k)
         torso_i = self.jr.robot.link("torso").id()

gtdynamics.i

@@ -16,24 +16,19 @@ const gtsam::KeyFormatter GTDKeyFormatter;
 
 /********************** factors **********************/
 #include <gtdynamics/factors/JointMeasurementFactor.h>
-template <JOINT>
 class JointMeasurementFactor : gtsam::NonlinearFactor {
   JointMeasurementFactor(gtsam::Key wTp_key, gtsam::Key wTc_key,
                          const gtsam::noiseModel::Base *cost_model,
                          const gtdynamics::Joint *joint,
-                         const JOINT::JointCoordinate &joint_coordinate,
-                         size_t k);
+                         double joint_coordinate);
   JointMeasurementFactor(const gtsam::noiseModel::Base::shared_ptr &model,
                          const gtdynamics::Joint *joint,
-                         const JOINT::JointCoordinate &joint_coordinate,
-                         size_t k);
+                         double joint_coordinate, size_t k);
 
   void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter =
                                        gtdynamics::GTDKeyFormatter);
 };
 
-typedef gtdynamics::JointMeasurementFactor<gtdynamics::RevoluteJoint> RevoluteJointMeasurementFactor;
-
 #include <gtdynamics/factors/PoseFactor.h>
 class PoseFactor : gtsam::NonlinearFactor {
   PoseFactor(gtsam::Key wTp_key, gtsam::Key wTc_key, gtsam::Key q_key,
@@ -104,10 +99,11 @@ class TwistFactor : gtsam::NonlinearFactor {
 
 #include <gtdynamics/factors/TwistAccelFactor.h>
 class TwistAccelFactor : gtsam::NonlinearFactor {
-  TwistAccelFactor(gtsam::Key twist_key_c, gtsam::Key twistAccel_key_p, gtsam::Key twistAccel_key_c,
-              gtsam::Key q_key, gtsam::Key qVel_key, gtsam::Key qAccel_key,
-              const gtsam::noiseModel::Base* cost_model,
-              const gtdynamics::JointTyped* joint);
+  TwistAccelFactor(gtsam::Key twist_key_c, gtsam::Key twistAccel_key_p,
+                   gtsam::Key twistAccel_key_c, gtsam::Key q_key,
+                   gtsam::Key qVel_key, gtsam::Key qAccel_key,
+                   const gtsam::noiseModel::Base *cost_model,
+                   const gtdynamics::Joint *joint);
 
   void print(const string &s="",
              const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
@@ -116,7 +112,7 @@ class TwistAccelFactor : gtsam::NonlinearFactor {
 #include <gtdynamics/factors/TorqueFactor.h>
 class TorqueFactor : gtsam::NonlinearFactor {
   TorqueFactor(const gtsam::noiseModel::Base *cost_model,
-               const gtdynamics::JointTyped *joint, size_t k=0);
+               const gtdynamics::Joint *joint, size_t k=0);
 
   void print(const string &s="",
              const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
@@ -145,15 +141,15 @@ class WrenchFactor : gtsam::NonlinearFactor {
 #include <gtdynamics/factors/WrenchEquivalenceFactor.h>
 class WrenchEquivalenceFactor : gtsam::NonlinearFactor{
   WrenchEquivalenceFactor(const gtsam::noiseModel::Base *cost_model,
-                          gtdynamics::JointTyped *joint, size_t k = 0);
+                          gtdynamics::Joint *joint, size_t k = 0);
   void print(const string &s="",
              const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
 };
 
 #include <gtdynamics/factors/WrenchPlanarFactor.h>
 class WrenchPlanarFactor : gtsam::NonlinearFactor {
   WrenchPlanarFactor(const gtsam::noiseModel::Base *cost_model,
-                     Vector planar_axis, gtdynamics::JointTyped *joint, size_t k=0);
+                     Vector planar_axis, gtdynamics::Joint *joint, size_t k=0);
   void print(const string &s="",
              const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
 };
@@ -231,11 +227,9 @@ class Link  {
 
 /********************** joint **********************/
 #include <gtdynamics/universal_robot/Joint.h>
-#include <gtdynamics/universal_robot/JointTyped.h>
-#include <gtdynamics/universal_robot/ScrewJointBase.h>
 #include <gtdynamics/universal_robot/RevoluteJoint.h>
 #include <gtdynamics/universal_robot/PrismaticJoint.h>
-#include <gtdynamics/universal_robot/ScrewJoint.h>
+#include <gtdynamics/universal_robot/HelicalJoint.h>
 class JointParams {
   JointParams();
   double velocity_limit;
@@ -259,12 +253,7 @@ virtual class Joint {
   gtdynamics::Link* child() const;
 };
 
-virtual class JointTyped : gtdynamics::Joint {
-};
-
-virtual class ScrewJointBase : gtdynamics::JointTyped {};
-
-virtual class RevoluteJoint : gtdynamics::ScrewJointBase {
+virtual class RevoluteJoint : gtdynamics::Joint {
   RevoluteJoint(
       int id, const string &name, const gtsam::Pose3 &wTj,
       const gtdynamics::Link *parent_link, const gtdynamics::Link *child_link,
@@ -273,7 +262,7 @@ virtual class RevoluteJoint : gtdynamics::ScrewJointBase {
   void print(const string &s = "") const;
 };
 
-virtual class PrismaticJoint : gtdynamics::ScrewJointBase {
+virtual class PrismaticJoint : gtdynamics::Joint {
   PrismaticJoint(
       int id, const string &name, const gtsam::Pose3 &wTj,
       const gtdynamics::Link *parent_link, const gtdynamics::Link *child_link,
@@ -282,8 +271,8 @@ virtual class PrismaticJoint : gtdynamics::ScrewJointBase {
   void print(const string &s = "") const;
 };
 
-virtual class ScrewJoint : gtdynamics::ScrewJointBase {
-  ScrewJoint(
+virtual class HelicalJoint : gtdynamics::Joint {
+  HelicalJoint(
       int id, const string &name, const gtsam::Pose3 &wTj,
       const gtdynamics::Link *parent_link, const gtdynamics::Link *child_link,
       const Vector &axis, double thread_pitch,
@@ -720,49 +709,37 @@ gtdynamics::DynamicsSymbol PhaseKey(int k);
 gtdynamics::DynamicsSymbol TimeKey(int t);
 
 ///////////////////// Key Methods /////////////////////
-template<T = {double}>
-void InsertJointAngle(gtsam::Values@ values, int j, int t, T value);
+void InsertJointAngle(gtsam::Values@ values, int j, int t, double value);
 
-template<T = {double}>
-void InsertJointAngle(gtsam::Values @values, int j, T value);
+void InsertJointAngle(gtsam::Values @values, int j, double value);
 
 gtsam::Vector JointAngle(const gtsam::VectorValues &values, int j, int t);
 
-template<T = {double}>
-T JointAngle(const gtsam::Values &values, int j, int t);
+double JointAngle(const gtsam::Values &values, int j, int t);
 
-template<T = {double}>
-void InsertJointVel(gtsam::Values @values, int j, int t, T value);
+void InsertJointVel(gtsam::Values @values, int j, int t, double value);
 
-template<T = {double}>
-void InsertJointVel(gtsam::Values @values, int j, T value);
+void InsertJointVel(gtsam::Values @values, int j, double value);
 
 gtsam::Vector JointVel(const gtsam::VectorValues &values, int j, int t);
 
-template<T = {double}>
-T JointVel(const gtsam::Values &values, int j, int t);
+double JointVel(const gtsam::Values &values, int j, int t);
 
-template<T = {double}>
-void InsertJointAccel(gtsam::Values @values, int j, int t, T value);
+void InsertJointAccel(gtsam::Values @values, int j, int t, double value);
 
-template<T = {double}>
-void InsertJointAccel(gtsam::Values @values, int j, T value);
+void InsertJointAccel(gtsam::Values @values, int j, double value);
 
 gtsam::Vector JointAccel(const gtsam::VectorValues &values, int j, int t);
 
-template<T = {double}>
-T JointAccel(const gtsam::Values &values, int j, int t);
+double JointAccel(const gtsam::Values &values, int j, int t);
 
-template<T = {double}>
-void InsertTorque(gtsam::Values @values, int j, int t, T value);
+void InsertTorque(gtsam::Values @values, int j, int t, double value);
 
-template<T = {double}>
-void InsertTorque(gtsam::Values @values, int j, T value);
+void InsertTorque(gtsam::Values @values, int j, double value);
 
 gtsam::Vector Torque(const gtsam::VectorValues &values, int j, int t);
 
-template<T = {double}>
-T Torque(const gtsam::Values &values, int j, int t);
+double Torque(const gtsam::Values &values, int j, int t);
 
 void InsertPose(gtsam::Values @values, int i, int t, gtsam::Pose3 value);
 

gtdynamics/cablerobot/src/cdpr_planar_sim.py

@@ -76,8 +76,8 @@ def update_kinematics(cdpr, fg, x, k):
                            Vs=[gtd.Twist(x, cdpr.ee_id(), k)]))
         # IK initial estimate
         for j in range(4):
-            gtd.InsertJointAngleDouble(x, j, k, 0)
-            gtd.InsertJointVelDouble(x, j, k, 0)
+            gtd.InsertJointAngle(x, j, k, 0)
+            gtd.InsertJointVel(x, j, k, 0)
         # IK solve
         result = gtsam.LevenbergMarquardtOptimizer(fg, x).optimize()
         assert abs(fg.error(result)) < 1e-20, "inverse kinematics didn't converge"
@@ -108,10 +108,10 @@ def update_dynamics(cdpr, fg, x, u, k, dt):
         fg.push_back(cdpr.collocation_factors(ks=[k], dt=dt))
         # ID priors (torque inputs)
         fg.push_back(
-            cdpr.priors_id(ks=[k], torquess=[[gtd.TorqueDouble(u, ji, k) for ji in range(4)]]))
+            cdpr.priors_id(ks=[k], torquess=[[gtd.Torque(u, ji, k) for ji in range(4)]]))
         # ID initial guess
         for ji in range(4):
-            gtd.InsertTorqueDouble(x, ji, k, gtd.TorqueDouble(u, ji, k))
+            gtd.InsertTorque(x, ji, k, gtd.Torque(u, ji, k))
             gtd.InsertWrench(x, cdpr.ee_id(), ji, k, np.zeros(6))
         gtd.InsertPose(x, cdpr.ee_id(), k+1, gtsam.Pose3(gtsam.Rot3(), (1.5, 0, 1.5)))
         gtd.InsertTwist(x, cdpr.ee_id(), k+1, np.zeros(6))
@@ -144,7 +144,7 @@ def step(self, verbose=False):
         u = self.controller.update(self.x, self.k)
         if verbose:
             print('control torques: {:.2e},   {:.2e},   {:.2e},   {:.2e}'.format(
-                *[gtd.TorqueDouble(u, ji, self.k) for ji in range(4)]))
+                *[gtd.Torque(u, ji, self.k) for ji in range(4)]))
         self.update_dynamics(self.cdpr, self.fg, self.x, u, self.k, self.dt)
         self.k += 1
         return self.x

gtdynamics/cablerobot/src/gerry00_planar_sim.py

@@ -26,10 +26,10 @@ def __init__(self, N):
         self.N = N
     def update(self, values, t):
         tau = gtsam.Values()
-        gtd.InsertTorqueDouble(tau, 0, t, 3 * np.cos(2. * t / self.N * np.pi))
-        gtd.InsertTorqueDouble(tau, 1, t, 3 * np.cos(2. * t / self.N * np.pi + np.pi/2))
-        gtd.InsertTorqueDouble(tau, 2, t, 3 * np.cos(2. * t / self.N * np.pi + np.pi))
-        gtd.InsertTorqueDouble(tau, 3, t, 3 * np.cos(2. * t / self.N * np.pi + 3*np.pi/2))
+        gtd.InsertTorque(tau, 0, t, 3 * np.cos(2. * t / self.N * np.pi))
+        gtd.InsertTorque(tau, 1, t, 3 * np.cos(2. * t / self.N * np.pi + np.pi/2))
+        gtd.InsertTorque(tau, 2, t, 3 * np.cos(2. * t / self.N * np.pi + np.pi))
+        gtd.InsertTorque(tau, 3, t, 3 * np.cos(2. * t / self.N * np.pi + 3*np.pi/2))
         return tau
 
 def main():