diff --git a/examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py b/examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py index ed0dc2e7..e9019f83 100644 --- a/examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py +++ b/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) diff --git a/examples/example_jumping_robot/src/jr_simulator.py b/examples/example_jumping_robot/src/jr_simulator.py index 59e9697c..e3243cbd 100644 --- a/examples/example_jumping_robot/src/jr_simulator.py +++ b/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) diff --git a/examples/example_jumping_robot/src/jr_values.py b/examples/example_jumping_robot/src/jr_values.py index 9c03414d..2cac38ce 100644 --- a/examples/example_jumping_robot/src/jr_values.py +++ b/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): diff --git a/examples/example_jumping_robot/src/jr_visualizer.py b/examples/example_jumping_robot/src/jr_visualizer.py index 781806b2..287b2036 100644 --- a/examples/example_jumping_robot/src/jr_visualizer.py +++ b/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))) diff --git a/examples/example_jumping_robot/tests/test_jr_simulator.py b/examples/example_jumping_robot/tests/test_jr_simulator.py index 2fb6ad44..2c7aff72 100644 --- a/examples/example_jumping_robot/tests/test_jr_simulator.py +++ b/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 diff --git a/examples/example_jumping_robot/tests/test_jumping_robot.py b/examples/example_jumping_robot/tests/test_jumping_robot.py index 30e279e2..4d910262 100644 --- a/examples/example_jumping_robot/tests/test_jumping_robot.py +++ b/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() diff --git a/gtdynamics.i b/gtdynamics.i index adb9860d..b6bca487 100644 --- a/gtdynamics.i +++ b/gtdynamics.i @@ -16,24 +16,19 @@ const gtsam::KeyFormatter GTDKeyFormatter; /********************** factors **********************/ #include -template 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 RevoluteJointMeasurementFactor; - #include 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 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 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,7 +141,7 @@ class WrenchFactor : gtsam::NonlinearFactor { #include 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); }; @@ -153,7 +149,7 @@ class WrenchEquivalenceFactor : gtsam::NonlinearFactor{ #include 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 -#include -#include #include #include -#include +#include 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 -void InsertJointAngle(gtsam::Values@ values, int j, int t, T value); +void InsertJointAngle(gtsam::Values@ values, int j, int t, double value); -template -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 JointAngle(const gtsam::Values &values, int j, int t); +double JointAngle(const gtsam::Values &values, int j, int t); -template -void InsertJointVel(gtsam::Values @values, int j, int t, T value); +void InsertJointVel(gtsam::Values @values, int j, int t, double value); -template -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 JointVel(const gtsam::Values &values, int j, int t); +double JointVel(const gtsam::Values &values, int j, int t); -template -void InsertJointAccel(gtsam::Values @values, int j, int t, T value); +void InsertJointAccel(gtsam::Values @values, int j, int t, double value); -template -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 JointAccel(const gtsam::Values &values, int j, int t); +double JointAccel(const gtsam::Values &values, int j, int t); -template -void InsertTorque(gtsam::Values @values, int j, int t, T value); +void InsertTorque(gtsam::Values @values, int j, int t, double value); -template -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 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); diff --git a/gtdynamics/cablerobot/src/cdpr_planar_sim.py b/gtdynamics/cablerobot/src/cdpr_planar_sim.py index ad6349ca..ad6846f6 100644 --- a/gtdynamics/cablerobot/src/cdpr_planar_sim.py +++ b/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 diff --git a/gtdynamics/cablerobot/src/gerry00_planar_sim.py b/gtdynamics/cablerobot/src/gerry00_planar_sim.py index c633e1c3..84b7f989 100644 --- a/gtdynamics/cablerobot/src/gerry00_planar_sim.py +++ b/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(): diff --git a/gtdynamics/cablerobot/src/test_cdpr_planar.py b/gtdynamics/cablerobot/src/test_cdpr_planar.py index 44fc3095..310fabfd 100644 --- a/gtdynamics/cablerobot/src/test_cdpr_planar.py +++ b/gtdynamics/cablerobot/src/test_cdpr_planar.py @@ -32,16 +32,16 @@ def testKinematics(self): def zeroValues(): values = gtsam.Values() for j in range(4): - gtd.InsertJointAngleDouble(values, j, 0, 3.0) - gtd.InsertJointVelDouble(values, j, 0, 17.0 + np.random.rand(1)) + gtd.InsertJointAngle(values, j, 0, 3.0) + gtd.InsertJointVel(values, j, 0, 17.0 + np.random.rand(1)) gtd.InsertPose(values, cdpr.ee_id(), 0, Pose3()) gtd.InsertTwist(values, cdpr.ee_id(), 0, (0,0,0,0,0,1.4142)) return values # things needed to define FK values = gtsam.Values() for j, l, ldot in zip(range(4), [1.35 * np.sqrt(2),]*4, [-1, 1, 1, -1.]): - gtd.InsertJointAngleDouble(values, j, 0, l) - gtd.InsertJointVelDouble(values, j, 0, ldot) + gtd.InsertJointAngle(values, j, 0, l) + gtd.InsertJointVel(values, j, 0, ldot) # things needed to define IK gtd.InsertPose(values, cdpr.ee_id(), 0, Pose3(Rot3(), (1.5, 0, 1.5))) gtd.InsertTwist(values, cdpr.ee_id(), 0, (0, 0, 0, 0, 0, -np.sqrt(2))) @@ -56,8 +56,8 @@ def zeroValues(): # try optimizing FK fkgraph = gtsam.NonlinearFactorGraph(kfg) fkgraph.push_back(cdpr.priors_fk([0], - [[gtd.JointAngleDouble(values, ji, 0) for ji in range(4)]], - [[gtd.JointVelDouble(values, ji, 0) for ji in range(4)]])) + [[gtd.JointAngle(values, ji, 0) for ji in range(4)]], + [[gtd.JointVel(values, ji, 0) for ji in range(4)]])) params = gtsam.LevenbergMarquardtParams() params.setAbsoluteErrorTol(1e-20) # FK less sensitive so we need to decrease the tolerance fkres = gtsam.LevenbergMarquardtOptimizer(fkgraph, zeroValues(), params).optimize() @@ -74,7 +74,7 @@ def testDynamicsInstantaneous(self): gtd.InsertTwist(values, cdpr.ee_id(), 0, np.zeros(6)) # things needed to define ID for j, tau in zip(range(4), [1, 0, 0, 1]): - gtd.InsertTorqueDouble(values, j, 0, tau) + gtd.InsertTorque(values, j, 0, tau) # things needed to define FD gtd.InsertTwistAccel(values, cdpr.ee_id(), 0, (0, 0, 0, 0, 0, -np.sqrt(2))) # things needed intermediaries @@ -102,7 +102,7 @@ def testDynamicsInstantaneous(self): init = gtsam.Values(values) for ji in range(4): init.erase(gtd.internal.TorqueKey(ji, 0).key()) - gtd.InsertTorqueDouble(init, ji, 0, -1) + gtd.InsertTorque(init, ji, 0, -1) results = gtsam.LevenbergMarquardtOptimizer(dfg, init).optimize() self.gtsamAssertEquals(results, values) @@ -126,9 +126,9 @@ def testDynamicsCollocation(self): init.insert(0, 0.01) for t in range(3): for j in range(4): - gtd.InsertJointAngleDouble(init, j, t, 1) - gtd.InsertJointVelDouble(init, j, t, 1) - gtd.InsertTorqueDouble(init, j, t, 1) + gtd.InsertJointAngle(init, j, t, 1) + gtd.InsertJointVel(init, j, t, 1) + gtd.InsertTorque(init, j, t, 1) gtd.InsertWrench(init, cdpr.ee_id(), j, t, np.ones(6)) gtd.InsertPose(init, cdpr.ee_id(), t, Pose3(Rot3(), (1.5, 1, 1.5))) gtd.InsertTwist(init, cdpr.ee_id(), t, np.ones(6)) diff --git a/gtdynamics/cablerobot/src/test_cdpr_planar_controller.py b/gtdynamics/cablerobot/src/test_cdpr_planar_controller.py index 6376efe2..7a0e55de 100644 --- a/gtdynamics/cablerobot/src/test_cdpr_planar_controller.py +++ b/gtdynamics/cablerobot/src/test_cdpr_planar_controller.py @@ -45,7 +45,7 @@ def testTrajFollow(self): print(('k: {:d} -- des: {:.3f}, {:.3f}, {:.3f} -- act: {:.3f}, {:.3f}, {:.3f}' + ' -- u: {:.3e}, {:.3e}, {:.3e}, {:.3e}').format( k, *des.translation(), *act.translation(), - *[gtd.TorqueDouble(result, ji, k) for ji in range(4)])) + *[gtd.Torque(result, ji, k) for ji in range(4)])) for k, (des, act) in enumerate(zip(x_des, pAct)): self.gtsamAssertEquals(des, act, tol=1e-2) diff --git a/gtdynamics/cablerobot/src/test_cdpr_planar_sim.py b/gtdynamics/cablerobot/src/test_cdpr_planar_sim.py index 78181247..6307e9fa 100644 --- a/gtdynamics/cablerobot/src/test_cdpr_planar_sim.py +++ b/gtdynamics/cablerobot/src/test_cdpr_planar_sim.py @@ -29,10 +29,10 @@ def testSim(self): class DummyController(CdprControllerBase): def update(self, values, k): tau = gtsam.Values() - gtd.InsertTorqueDouble(tau, 0, k, 1.) - gtd.InsertTorqueDouble(tau, 1, k, 1.) - gtd.InsertTorqueDouble(tau, 2, k, 0.) - gtd.InsertTorqueDouble(tau, 3, k, 0.) + gtd.InsertTorque(tau, 0, k, 1.) + gtd.InsertTorque(tau, 1, k, 1.) + gtd.InsertTorque(tau, 2, k, 0.) + gtd.InsertTorque(tau, 3, k, 0.) return tau Tf = 1 dt = 0.1 diff --git a/gtdynamics/cablerobot/src/utils.py b/gtdynamics/cablerobot/src/utils.py index 547d72d6..2e346702 100644 --- a/gtdynamics/cablerobot/src/utils.py +++ b/gtdynamics/cablerobot/src/utils.py @@ -29,9 +29,9 @@ def zerovalues(lid, ts=[], dt=0.01): zero.insert(0, dt) for t in ts: for j in range(4): - gtd.InsertJointAngleDouble(zero, j, t, 0) - gtd.InsertJointVelDouble(zero, j, t, 0) - gtd.InsertTorqueDouble(zero, j, t, 0) + gtd.InsertJointAngle(zero, j, t, 0) + gtd.InsertJointVel(zero, j, t, 0) + gtd.InsertTorque(zero, j, t, 0) gtd.InsertWrench(zero, lid, j, t, np.zeros(6)) gtd.InsertPose(zero, lid, t, gtsam.Pose3(gtsam.Rot3(), (1.5, 0, 1.5))) gtd.InsertTwist(zero, lid, t, np.zeros(6)) diff --git a/gtdynamics/dynamics/DynamicsGraph.cpp b/gtdynamics/dynamics/DynamicsGraph.cpp index 9c1010e0..448fa182 100644 --- a/gtdynamics/dynamics/DynamicsGraph.cpp +++ b/gtdynamics/dynamics/DynamicsGraph.cpp @@ -269,12 +269,12 @@ gtsam::NonlinearFactorGraph DynamicsGraph::aFactors( gtsam::Z_6x1, opt_.ba_cost_model); for (auto &&joint : robot.joints()) graph.emplace_shared( - internal::TwistKey(joint->child()->id(), t), - internal::TwistAccelKey(joint->parent()->id(), t), - internal::TwistAccelKey(joint->child()->id(), t), - internal::JointAngleKey(joint->id(), t), internal::JointVelKey(joint->id(), t), - internal::JointAccelKey(joint->id(), t), opt_.a_cost_model, - boost::static_pointer_cast(joint)); + internal::TwistKey(joint->child()->id(), t), + internal::TwistAccelKey(joint->parent()->id(), t), + internal::TwistAccelKey(joint->child()->id(), t), + internal::JointAngleKey(joint->id(), t), + internal::JointVelKey(joint->id(), t), + internal::JointAccelKey(joint->id(), t), opt_.a_cost_model, joint); // Add contact factors. if (contact_points) { @@ -351,7 +351,7 @@ gtsam::NonlinearFactorGraph DynamicsGraph::dynamicsFactors( // TODO(frank): sort out const shared ptr mess for (auto &&joint : robot.joints()) { auto j = joint->id(), child_id = joint->child()->id(); - auto const_joint = boost::static_pointer_cast(joint); + auto const_joint = joint; graph.emplace_shared(opt_.f_cost_model, const_joint, k); graph.emplace_shared(opt_.t_cost_model, const_joint, k); diff --git a/gtdynamics/dynamics/Simulator.h b/gtdynamics/dynamics/Simulator.h index 65da17ef..4fe5a8d1 100644 --- a/gtdynamics/dynamics/Simulator.h +++ b/gtdynamics/dynamics/Simulator.h @@ -93,9 +93,9 @@ class Simulator { const double dt2 = std::pow(dt, 2); for (auto &&joint : robot_.joints()) { auto j = joint->id(); - const double q = JointAngle(current_values_, j); - const double v = JointVel(current_values_, j); - const double a = JointAccel(current_values_, j); + const double q = JointAngle(current_values_, j); + const double v = JointVel(current_values_, j); + const double a = JointAccel(current_values_, j); // TODO(frank): one could use t values and save entire simulation. const double v_new = v + dt * a; diff --git a/gtdynamics/factors/JointLimitFactor.h b/gtdynamics/factors/JointLimitFactor.h index 4cbc5638..712e451a 100644 --- a/gtdynamics/factors/JointLimitFactor.h +++ b/gtdynamics/factors/JointLimitFactor.h @@ -24,7 +24,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" namespace gtdynamics { @@ -32,13 +32,11 @@ namespace gtdynamics { * JointLimitFactor is a class which enforces joint angle, velocity, * acceleration and torque value to be within limi */ -class JointLimitFactor - : public gtsam::NoiseModelFactor1 { +class JointLimitFactor : public gtsam::NoiseModelFactor1 { private: - using JointCoordinateType = JointTyped::JointCoordinateType; using This = JointLimitFactor; - using Base = gtsam::NoiseModelFactor1; - JointCoordinateType low_, high_; + using Base = gtsam::NoiseModelFactor1; + double low_, high_; public: /** @@ -51,9 +49,8 @@ class JointLimitFactor */ JointLimitFactor(gtsam::Key q_key, const gtsam::noiseModel::Base::shared_ptr &cost_model, - JointCoordinateType lower_limit, - JointCoordinateType upper_limit, - JointCoordinateType limit_threshold) + double lower_limit, double upper_limit, + double limit_threshold) : Base(cost_model, q_key), low_(lower_limit + limit_threshold), high_(upper_limit - limit_threshold) {} @@ -72,7 +69,7 @@ class JointLimitFactor * @param q joint value */ gtsam::Vector evaluateError( - const JointCoordinateType &q, + const double &q, boost::optional H_q = boost::none) const override { if (q < low_) { if (H_q) *H_q = -gtsam::I_1x1; diff --git a/gtdynamics/factors/JointMeasurementFactor.h b/gtdynamics/factors/JointMeasurementFactor.h index 57ce6e3b..79a2711e 100644 --- a/gtdynamics/factors/JointMeasurementFactor.h +++ b/gtdynamics/factors/JointMeasurementFactor.h @@ -23,10 +23,7 @@ namespace gtdynamics { /** * @brief A 2-way factor to relate the parent and child links of a joint given * the joint coordinate as a measurement. - * - * @tparam JOINT The type of joint which is being constrained. */ -template class JointMeasurementFactor : public gtsam::NoiseModelFactor2 { private: @@ -34,8 +31,7 @@ class JointMeasurementFactor using Base = gtsam::NoiseModelFactor2; JointConstSharedPtr joint_; - typename JOINT::JointCoordinate joint_coordinate_; - size_t k_; + double measured_joint_coordinate_; public: /** @@ -48,15 +44,13 @@ class JointMeasurementFactor * @param joint_coordinate The coordinates of the joint motion. * @param k The time index. */ - JointMeasurementFactor( - gtsam::Key wTp_key, gtsam::Key wTc_key, - const gtsam::noiseModel::Base::shared_ptr& model, - const JointConstSharedPtr joint, - const typename JOINT::JointCoordinate& joint_coordinate, size_t k) + JointMeasurementFactor(gtsam::Key wTp_key, gtsam::Key wTc_key, + const gtsam::noiseModel::Base::shared_ptr& model, + const JointConstSharedPtr joint, + double joint_coordinate) : Base(model, wTp_key, wTc_key), joint_(joint), - joint_coordinate_(joint_coordinate), - k_(k) {} + measured_joint_coordinate_(joint_coordinate) {} /** * @brief Convenience constructor @@ -66,26 +60,21 @@ class JointMeasurementFactor * @param joint_coordinate The coordinates of the joint motion. * @param k The time index. */ - JointMeasurementFactor( - const gtsam::noiseModel::Base::shared_ptr& model, - const JointConstSharedPtr joint, - const typename JOINT::JointCoordinate& joint_coordinate, size_t k) + JointMeasurementFactor(const gtsam::noiseModel::Base::shared_ptr& model, + const JointConstSharedPtr joint, + double joint_coordinate, size_t k) : Base(model, internal::PoseKey(joint->parent()->id(), k), internal::PoseKey(joint->child()->id(), k)), joint_(joint), - joint_coordinate_(joint_coordinate), - k_(k) {} + measured_joint_coordinate_(joint_coordinate) {} gtsam::Vector evaluateError( const gtsam::Pose3& wTp, const gtsam::Pose3& wTc, boost::optional H_wTp = boost::none, boost::optional H_wTc = boost::none) const override { - gtsam::Values joint_angles; - InsertJointAngle(&joint_angles, joint_->id(), k_, joint_coordinate_); - gtsam::Matrix6 H; gtsam::Pose3 wTc_hat = - joint_->poseOf(joint_->child(), wTp, joint_angles, k_, H_wTp); + joint_->poseOf(joint_->child(), wTp, measured_joint_coordinate_, H_wTp); gtsam::Vector6 error = wTc.logmap(wTc_hat, H_wTc, H_wTp ? &H : 0); if (H_wTp) { @@ -100,8 +89,7 @@ class JointMeasurementFactor gtsam::DefaultKeyFormatter) const override { std::cout << s << "JointMeasurementFactor(" << keyFormatter(key1()) << "," << keyFormatter(key2()) << ")\n"; - gtsam::traits::Print(joint_coordinate_, - " measured: "); + gtsam::traits::Print(measured_joint_coordinate_, " measured: "); this->noiseModel_->print(" noise model: "); } }; diff --git a/gtdynamics/factors/PoseFactor.h b/gtdynamics/factors/PoseFactor.h index 4e9c72ad..6a2a172b 100644 --- a/gtdynamics/factors/PoseFactor.h +++ b/gtdynamics/factors/PoseFactor.h @@ -106,8 +106,9 @@ class PoseFactor : public gtsam::NoiseModelFactor { boost::optional wTc_hat_H_q_ref; if (H) wTc_hat_H_q_ref = wTc_hat_H_q; - auto wTc_hat = joint_->poseOf(joint_->child(), wTp, x, t_, - H ? &wTc_hat_H_wTp : 0, wTc_hat_H_q_ref); + auto wTc_hat = + joint_->poseOf(joint_->child(), wTp, JointAngle(x, joint_->id(), t_), + H ? &wTc_hat_H_wTp : 0, wTc_hat_H_q_ref); gtsam::Vector6 error = wTc.logmap(wTc_hat, H ? &H_wTc : 0, H ? &H_wTc_hat : 0); if (H) { diff --git a/gtdynamics/factors/PreintegratedContactFactors.h b/gtdynamics/factors/PreintegratedContactFactors.h index da5891cf..7b1d4c3b 100644 --- a/gtdynamics/factors/PreintegratedContactFactors.h +++ b/gtdynamics/factors/PreintegratedContactFactors.h @@ -24,7 +24,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" #include "gtdynamics/universal_robot/Link.h" namespace gtdynamics { diff --git a/gtdynamics/factors/TorqueFactor.h b/gtdynamics/factors/TorqueFactor.h index 2b0a9a91..4375b6e8 100644 --- a/gtdynamics/factors/TorqueFactor.h +++ b/gtdynamics/factors/TorqueFactor.h @@ -21,7 +21,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/utils/values.h" @@ -31,20 +31,16 @@ namespace gtdynamics { * TorqueFactor is a two-way nonlinear factor which enforces relation between * wrench and torque on each link */ -class TorqueFactor - : public gtsam::NoiseModelFactor2 { +class TorqueFactor : public gtsam::NoiseModelFactor2 { private: - using JointTorque = typename JointTyped::JointTorque; using This = TorqueFactor; - using Base = gtsam::NoiseModelFactor2; - using MyJointConstSharedPtr = boost::shared_ptr; - MyJointConstSharedPtr joint_; + using Base = gtsam::NoiseModelFactor2; + JointConstSharedPtr joint_; /// Private constructor with arbitrary keys TorqueFactor(const gtsam::noiseModel::Base::shared_ptr &cost_model, gtsam::Key wrench_key, gtsam::Key torque_key, - const MyJointConstSharedPtr &joint) + const JointConstSharedPtr &joint) : Base(cost_model, wrench_key, torque_key), joint_(joint) {} public: @@ -58,7 +54,7 @@ class TorqueFactor * @param joint JointConstSharedPtr to the joint */ TorqueFactor(const gtsam::noiseModel::Base::shared_ptr &cost_model, - const MyJointConstSharedPtr &joint, size_t k = 0) + const JointConstSharedPtr &joint, size_t k = 0) : TorqueFactor(cost_model, internal::WrenchKey(joint->child()->id(), joint->id(), k), internal::TorqueKey(joint->id(), k), joint) {} @@ -72,11 +68,11 @@ class TorqueFactor * @param torque torque on this link joint */ gtsam::Vector evaluateError( - const gtsam::Vector6 &wrench, const JointTorque &torque, + const gtsam::Vector6 &wrench, const double &torque, boost::optional H_wrench = boost::none, boost::optional H_torque = boost::none) const override { if (H_torque) { - *H_torque = -JointTyped::MatrixN::Identity(); + *H_torque = -gtsam::Matrix1::Identity(); } // TODO(G+S): next PR will generalize this from Vector1 return gtsam::Vector1( @@ -85,7 +81,7 @@ class TorqueFactor } /// Returns the joint - MyJointConstSharedPtr getJoint() const { return joint_; } + JointConstSharedPtr getJoint() const { return joint_; } //// @return a deep copy of this factor gtsam::NonlinearFactor::shared_ptr clone() const override { diff --git a/gtdynamics/factors/TwistAccelFactor.h b/gtdynamics/factors/TwistAccelFactor.h index 19bf21f9..d7c42006 100644 --- a/gtdynamics/factors/TwistAccelFactor.h +++ b/gtdynamics/factors/TwistAccelFactor.h @@ -23,7 +23,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" namespace gtdynamics { @@ -32,20 +32,14 @@ namespace gtdynamics { * between acceleration on previous link and this link. */ class TwistAccelFactor - : public gtsam::NoiseModelFactor6< - gtsam::Vector6, gtsam::Vector6, gtsam::Vector6, - JointTyped::JointCoordinate, JointTyped::JointVelocity, - JointTyped::JointAcceleration> { + : public gtsam::NoiseModelFactor6 { private: - using JointCoordinate = JointTyped::JointCoordinate; - using JointVelocity = JointTyped::JointVelocity; - using JointAcceleration = JointTyped::JointVelocity; using This = TwistAccelFactor; using Base = gtsam::NoiseModelFactor6; - using JointTypedConstSharedPtr = boost::shared_ptr; - JointTypedConstSharedPtr joint_; + gtsam::Vector6, double, + double, double>; + JointConstSharedPtr joint_; public: /** @@ -61,7 +55,7 @@ class TwistAccelFactor gtsam::Key twistAccel_key_c, gtsam::Key q_key, gtsam::Key qVel_key, gtsam::Key qAccel_key, const gtsam::noiseModel::Base::shared_ptr &cost_model, - JointTypedConstSharedPtr joint) + JointConstSharedPtr joint) : Base(cost_model, twist_key_c, twistAccel_key_p, twistAccel_key_c, q_key, qVel_key, qAccel_key), joint_(joint) {} @@ -80,8 +74,8 @@ class TwistAccelFactor */ gtsam::Vector evaluateError( const gtsam::Vector6 &twist_c, const gtsam::Vector6 &twistAccel_p, - const gtsam::Vector6 &twistAccel_c, const JointCoordinate &q, - const JointAcceleration &qVel, const JointAcceleration &qAccel, + const gtsam::Vector6 &twistAccel_c, const double &q, + const double &qVel, const double &qAccel, boost::optional H_twist_c = boost::none, boost::optional H_twistAccel_p = boost::none, boost::optional H_twistAccel_c = boost::none, diff --git a/gtdynamics/factors/TwistFactor.h b/gtdynamics/factors/TwistFactor.h index 497dbb3d..a4ef254a 100644 --- a/gtdynamics/factors/TwistFactor.h +++ b/gtdynamics/factors/TwistFactor.h @@ -21,7 +21,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" namespace gtdynamics { @@ -30,15 +30,12 @@ namespace gtdynamics { * between twist on previous link and this link */ class TwistFactor - : public gtsam::NoiseModelFactor4 { + : public gtsam::NoiseModelFactor4 { private: - using JointCoordinate = typename JointTyped::JointCoordinate; - using JointVelocity = typename JointTyped::JointVelocity; using This = TwistFactor; - using Base = gtsam::NoiseModelFactor4; + using Base = + gtsam::NoiseModelFactor4; gtsam::Pose3 cMp_; JointConstSharedPtr joint_; @@ -70,15 +67,14 @@ class TwistFactor */ gtsam::Vector evaluateError( const gtsam::Vector6 &twist_p, const gtsam::Vector6 &twist_c, - const JointCoordinate &q, const JointVelocity &qVel, + const double &q, const double &qVel, boost::optional H_twist_p = boost::none, boost::optional H_twist_c = boost::none, boost::optional H_q = boost::none, boost::optional H_qVel = boost::none) const override { - auto error = - boost::static_pointer_cast(joint_)->transformTwistTo( - joint_->child(), q, qVel, twist_p, H_q, H_qVel, H_twist_p) - - twist_c; + auto error = joint_->transformTwistTo(joint_->child(), q, qVel, twist_p, + H_q, H_qVel, H_twist_p) - + twist_c; if (H_twist_c) { *H_twist_c = -gtsam::I_6x6; diff --git a/gtdynamics/factors/WrenchEquivalenceFactor.h b/gtdynamics/factors/WrenchEquivalenceFactor.h index d719f19c..3e0020e3 100644 --- a/gtdynamics/factors/WrenchEquivalenceFactor.h +++ b/gtdynamics/factors/WrenchEquivalenceFactor.h @@ -24,7 +24,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/utils/values.h" @@ -33,22 +33,18 @@ namespace gtdynamics { /** WrenchEquivalenceFactor is a 3-way nonlinear factor which enforces * relation between wrench expressed in two link frames*/ class WrenchEquivalenceFactor - : public gtsam::NoiseModelFactor3 { + : public gtsam::NoiseModelFactor3 { private: - using JointCoordinate = typename JointTyped::JointCoordinate; using This = WrenchEquivalenceFactor; - using Base = - gtsam::NoiseModelFactor3; + using Base = gtsam::NoiseModelFactor3; - using JointTypedConstSharedPtr = boost::shared_ptr; - JointTypedConstSharedPtr joint_; + JointConstSharedPtr joint_; /// Private constructor with arbitrary keys WrenchEquivalenceFactor(const gtsam::noiseModel::Base::shared_ptr &cost_model, gtsam::Key wrench_key_1, gtsam::Key wrench_key_2, gtsam::Key q_key, - const JointTypedConstSharedPtr &joint) + const JointConstSharedPtr &joint) : Base(cost_model, wrench_key_1, wrench_key_2, q_key), joint_(joint) {} public: @@ -57,7 +53,7 @@ class WrenchEquivalenceFactor * @param joint JointConstSharedPtr to the joint */ WrenchEquivalenceFactor(const gtsam::noiseModel::Base::shared_ptr &cost_model, - const JointTypedConstSharedPtr &joint, size_t k = 0) + const JointConstSharedPtr &joint, size_t k = 0) : WrenchEquivalenceFactor( cost_model, internal::WrenchKey(joint->parent()->id(), joint->id(), k), @@ -75,11 +71,11 @@ class WrenchEquivalenceFactor */ gtsam::Vector evaluateError( const gtsam::Vector6 &wrench_1, const gtsam::Vector6 &wrench_2, - const JointCoordinate &q, + const double &q, boost::optional H_wrench_1 = boost::none, boost::optional H_wrench_2 = boost::none, boost::optional H_q = boost::none) const override { - gtsam::Pose3 T_21 = joint_->relativePoseOf(joint_->parent(), q); + gtsam::Pose3 T_21 = joint_->childTparent(q); gtsam::Matrix6 Ad_21_T = T_21.AdjointMap().transpose(); gtsam::Vector6 error = wrench_1 + Ad_21_T * wrench_2; @@ -91,9 +87,10 @@ class WrenchEquivalenceFactor } if (H_q) { // TODO(frank): really, child? Double-check derivatives - *H_q = - joint_->AdjointMapJacobianJointAngle(joint_->child(), q).transpose() * - wrench_2; + *H_q = AdjointMapJacobianQ(q, joint_->childTparent(0.0), + joint_->screwAxis(joint_->child())) + .transpose() * + wrench_2; } return error; } diff --git a/gtdynamics/factors/WrenchPlanarFactor.h b/gtdynamics/factors/WrenchPlanarFactor.h index e94537dd..78810033 100644 --- a/gtdynamics/factors/WrenchPlanarFactor.h +++ b/gtdynamics/factors/WrenchPlanarFactor.h @@ -20,7 +20,7 @@ #include #include -#include "gtdynamics/universal_robot/JointTyped.h" +#include "gtdynamics/universal_robot/Joint.h" #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/utils/utils.h" #include "gtdynamics/utils/values.h" @@ -56,8 +56,7 @@ class WrenchPlanarFactor : public gtsam::NoiseModelFactor1 { */ WrenchPlanarFactor(const gtsam::noiseModel::Base::shared_ptr &cost_model, gtsam::Vector3 planar_axis, - const boost::shared_ptr &joint, - size_t k = 0) + const JointConstSharedPtr &joint, size_t k = 0) : WrenchPlanarFactor( cost_model, planar_axis, internal::WrenchKey(joint->child()->id(), joint->id(), k)) {} diff --git a/gtdynamics/statics/StaticsSlice.cpp b/gtdynamics/statics/StaticsSlice.cpp index 75f70409..92e68b8d 100644 --- a/gtdynamics/statics/StaticsSlice.cpp +++ b/gtdynamics/statics/StaticsSlice.cpp @@ -33,9 +33,8 @@ gtsam::NonlinearFactorGraph Statics::wrenchEquivalenceFactors( const Slice& slice, const Robot& robot) const { gtsam::NonlinearFactorGraph graph; for (auto&& joint : robot.joints()) { - graph.emplace_shared( - p_->f_cost_model, boost::static_pointer_cast(joint), - slice.k); + graph.emplace_shared(p_->f_cost_model, joint, + slice.k); } return graph; } @@ -44,9 +43,7 @@ gtsam::NonlinearFactorGraph Statics::torqueFactors(const Slice& slice, const Robot& robot) const { gtsam::NonlinearFactorGraph graph; for (auto&& joint : robot.joints()) { - graph.emplace_shared( - p_->t_cost_model, boost::static_pointer_cast(joint), - slice.k); + graph.emplace_shared(p_->t_cost_model, joint, slice.k); } return graph; } @@ -57,8 +54,7 @@ gtsam::NonlinearFactorGraph Statics::wrenchPlanarFactors( if (p_->planar_axis) for (auto&& joint : robot.joints()) { graph.emplace_shared( - p_->planar_cost_model, *p_->planar_axis, - boost::static_pointer_cast(joint), slice.k); + p_->planar_cost_model, *p_->planar_axis, joint, slice.k); } return graph; } diff --git a/gtdynamics/universal_robot/ScrewJoint.h b/gtdynamics/universal_robot/HelicalJoint.h similarity index 72% rename from gtdynamics/universal_robot/ScrewJoint.h rename to gtdynamics/universal_robot/HelicalJoint.h index 5601449f..fdcc1e72 100644 --- a/gtdynamics/universal_robot/ScrewJoint.h +++ b/gtdynamics/universal_robot/HelicalJoint.h @@ -6,7 +6,7 @@ * -------------------------------------------------------------------------- */ /** - * @file ScrewJoint.h + * @file HelicalJoint.h * @brief Representation of screw joint. * @author Frank Dellaert * @author Mandy Xie @@ -18,16 +18,15 @@ #pragma once -#include "gtdynamics/universal_robot/ScrewJointBase.h" +#include "gtdynamics/universal_robot/Joint.h" namespace gtdynamics { /** - * @class ScrewJoint is an implementation of the ScrewJointBase class - * which represents a screw joint and contains all necessary factor - * construction methods. + * @class HelicalJoint is an implementation of the Joint class which represents + * a helical joint and contains all necessary factor construction methods. */ -class ScrewJoint : public ScrewJointBase { +class HelicalJoint : public Joint { protected: /** * Returns the screw axis in the joint frame given the joint axis and thread @@ -41,7 +40,7 @@ class ScrewJoint : public ScrewJointBase { public: /** - * @brief Create ScrewJoint using JointParams, joint name, joint pose in + * @brief Create HelicalJoint using JointParams, joint name, joint pose in * world frame, screw axes, and parent and child links. * * @param[in] id id for keys @@ -53,15 +52,18 @@ class ScrewJoint : public ScrewJointBase { * @param[in] thread_pitch joint's thread pitch in dist per rev * @param[in] parameters JointParams struct. */ - ScrewJoint(uint8_t id, const std::string &name, const gtsam::Pose3 &bTj, + HelicalJoint(uint8_t id, const std::string &name, const gtsam::Pose3 &bTj, const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link, const gtsam::Vector3 &axis, double thread_pitch, const JointParams ¶meters = JointParams()) - : ScrewJointBase(id, name, bTj, parent_link, child_link, axis, - getScrewAxis(axis, thread_pitch), parameters) {} + : Joint(id, name, bTj, parent_link, child_link, + getScrewAxis(axis, thread_pitch), parameters) {} + + /// Constructor directly from screwAxis + using Joint::Joint; /// Return joint type for use in reconstructing robot from JointParams. - Type type() const override { return Type::Screw; } + Type type() const final override { return Type::Screw; } }; } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/Joint.cpp b/gtdynamics/universal_robot/Joint.cpp index 568378f2..75a90f99 100644 --- a/gtdynamics/universal_robot/Joint.cpp +++ b/gtdynamics/universal_robot/Joint.cpp @@ -19,21 +19,26 @@ #include #include "gtdynamics/universal_robot/Link.h" +#include "gtdynamics/factors/JointLimitFactor.h" + +using gtsam::Pose3; +using gtsam::Vector6; namespace gtdynamics { /* ************************************************************************* */ Joint::Joint(uint8_t id, const std::string &name, const Pose3 &bTj, const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link, - const JointParams ¶meters) + const Vector6 &jScrewAxis, const JointParams ¶meters) : id_(id), name_(name), parent_link_(parent_link), child_link_(child_link), - parameters_(parameters) { - jMp_ = bTj.inverse() * parent_link_->bMcom(); - jMc_ = bTj.inverse() * child_link_->bMcom(); -} + jMp_(bTj.inverse() * parent_link->bMcom()), + jMc_(bTj.inverse() * child_link->bMcom()), + pScrewAxis_(-jMp_.inverse().AdjointMap() * jScrewAxis), + cScrewAxis_(jMc_.inverse().AdjointMap() * jScrewAxis), + parameters_(parameters) {} /* ************************************************************************* */ bool Joint::isChildLink(const LinkSharedPtr &link) const { @@ -43,10 +48,239 @@ bool Joint::isChildLink(const LinkSharedPtr &link) const { return link == child_link_; } + +/* ************************************************************************* */ +Pose3 Joint::parentTchild( + double q, gtsam::OptionalJacobian<6, 1> pMc_H_q) const { + // Calculate pose of child in parent link, at rest. + // TODO(dellaert): store `pMj_` rather than `jMp_`. + // NOTE(dellaert): Only if this is called more often than childTparent. + const Pose3 pMc = jMp_.inverse() * jMc_; + + // Multiply screw axis with joint angle to get a finite 6D screw. + const Vector6 screw = cScrewAxis_ * q; + + // Calculate the actual relative pose taking into account the joint angle. + // TODO(dellaert): use formula `pMj_ * screw_around_Z * jMc_`. + gtsam::Matrix6 exp_H_screw; + const Pose3 exp = Pose3::Expmap(screw, pMc_H_q ? &exp_H_screw : 0); + if (pMc_H_q) { + *pMc_H_q = exp_H_screw * cScrewAxis_; + } + return pMc * exp; // Note: derivative of compose in exp is identity. +} + +/* ************************************************************************* */ +Pose3 Joint::childTparent( + double q, gtsam::OptionalJacobian<6, 1> cMp_H_q) const { + // TODO(frank): don't go via inverse, specialize in base class + Vector6 pMc_H_q; + Pose3 pMc = parentTchild(q, cMp_H_q ? &pMc_H_q : 0); // pMc(q) -> pMc_H_q + gtsam::Matrix6 cMp_H_pMc; + Pose3 cMp = pMc.inverse(cMp_H_q ? &cMp_H_pMc : 0); // cMp(pMc) -> cMp_H_pMc + if (cMp_H_q) { + *cMp_H_q = cMp_H_pMc * pMc_H_q; + } + return cMp; +} + +/* ************************************************************************* */ +Vector6 Joint::transformTwistTo( + const LinkSharedPtr &link, double q, double q_dot, + boost::optional other_twist, + gtsam::OptionalJacobian<6, 1> H_q, + gtsam::OptionalJacobian<6, 1> H_q_dot, + gtsam::OptionalJacobian<6, 6> H_other_twist) const { + Vector6 other_twist_ = other_twist ? *other_twist : Vector6::Zero(); + + auto other = otherLink(link); + auto this_ad_other = relativePoseOf(other, q).AdjointMap(); + + if (H_q) { + // TODO(frank): really, zero below? Check derivatives + *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screwAxis(link)) * + other_twist_; + } + if (H_q_dot) { + *H_q_dot = screwAxis(link); + } + if (H_other_twist) { + *H_other_twist = this_ad_other; + } + + return this_ad_other * other_twist_ + screwAxis(link) * q_dot; +} + +/* ************************************************************************* */ +Vector6 Joint::transformTwistAccelTo( + const LinkSharedPtr &link, double q, double q_dot, double q_ddot, + boost::optional this_twist, + boost::optional other_twist_accel, + gtsam::OptionalJacobian<6, 1> H_q, + gtsam::OptionalJacobian<6, 1> H_q_dot, + gtsam::OptionalJacobian<6, 1> H_q_ddot, + gtsam::OptionalJacobian<6, 6> H_this_twist, + gtsam::OptionalJacobian<6, 6> H_other_twist_accel) const { + Vector6 this_twist_ = this_twist ? *this_twist : Vector6::Zero(); + Vector6 other_twist_accel_ = + other_twist_accel ? *other_twist_accel : Vector6::Zero(); + Vector6 screw_axis_ = isChildLink(link) ? cScrewAxis_ : pScrewAxis_; + + // i = other link + // j = this link + auto other = otherLink(link); + Pose3 jTi = relativePoseOf(other, q); + + Vector6 this_twist_accel = + jTi.AdjointMap() * other_twist_accel_ + + Pose3::adjoint(this_twist_, screw_axis_ * q_dot, H_this_twist) + + screw_axis_ * q_ddot; + + if (H_other_twist_accel) { + *H_other_twist_accel = jTi.AdjointMap(); + } + if (H_q) { + // TODO(frank): really, zero below? Check derivatives. Also, copy/pasta from + // above? + *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screw_axis_) * + other_twist_accel_; + } + if (H_q_dot) { + *H_q_dot = Pose3::adjointMap(this_twist_) * screw_axis_; + } + if (H_q_ddot) { + *H_q_ddot = screw_axis_; + } + + return this_twist_accel; +} + +/* ************************************************************************* */ +double Joint::transformWrenchToTorque( + const LinkSharedPtr &link, boost::optional wrench, + gtsam::OptionalJacobian<1, 6> H_wrench) const { + auto screw_axis_ = screwAxis(link); + if (H_wrench) { + *H_wrench = screw_axis_.transpose(); + } + return screw_axis_.transpose() * (wrench ? *wrench : Vector6::Zero()); +} + +/* ************************************************************************* */ +gtsam::GaussianFactorGraph Joint::linearFDPriors( + size_t t, const gtsam::Values &known_values, + const OptimizerSetting &opt) const { + gtsam::GaussianFactorGraph priors; + gtsam::Vector1 rhs(Torque(known_values, id(), t)); + // TODO(alej`andro): use optimizer settings + priors.add(internal::TorqueKey(id(), t), gtsam::I_1x1, rhs, + gtsam::noiseModel::Constrained::All(1)); + return priors; +} + +/* ************************************************************************* */ +gtsam::GaussianFactorGraph Joint::linearAFactors( + size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, + const boost::optional &planar_axis) const { + gtsam::GaussianFactorGraph graph; + + const Pose3 T_wi1 = Pose(known_values, parent()->id(), t); + const Pose3 T_wi2 = Pose(known_values, child()->id(), t); + const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1; + const Vector6 V_i2 = Twist(known_values, child()->id(), t); + const Vector6 S_i2_j = screwAxis(child_link_); + const double v_j = JointVel(known_values, id(), t); + + // twist acceleration factor + // A_i2 - Ad(T_21) * A_i1 - S_i2_j * a_j = ad(V_i2) * S_i2_j * v_j + Vector6 rhs_tw = Pose3::adjointMap(V_i2) * S_i2_j * v_j; + graph.add(internal::TwistAccelKey(child()->id(), t), gtsam::I_6x6, + internal::TwistAccelKey(parent()->id(), t), -T_i2i1.AdjointMap(), + internal::JointAccelKey(id(), t), -S_i2_j, rhs_tw, + gtsam::noiseModel::Constrained::All(6)); + + return graph; +} + +/* ************************************************************************* */ +gtsam::GaussianFactorGraph Joint::linearDynamicsFactors( + size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, + const boost::optional &planar_axis) const { + gtsam::GaussianFactorGraph graph; + + const Pose3 T_wi1 = Pose(known_values, parent()->id(), t); + const Pose3 T_wi2 = Pose(known_values, child()->id(), t); + const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1; + const Vector6 S_i2_j = screwAxis(child_link_); + + // torque factor + // S_i_j^T * F_i_j - tau = 0 + gtsam::Vector1 rhs_torque = gtsam::Vector1::Zero(); + graph.add(internal::WrenchKey(child()->id(), id(), t), S_i2_j.transpose(), + internal::TorqueKey(id(), t), -gtsam::I_1x1, rhs_torque, + gtsam::noiseModel::Constrained::All(1)); + + // wrench equivalence factor + // F_i1_j + Ad(T_i2i1)^T F_i2_j = 0 + Vector6 rhs_weq = Vector6::Zero(); + graph.add(internal::WrenchKey(parent()->id(), id(), t), gtsam::I_6x6, + internal::WrenchKey(child()->id(), id(), t), + T_i2i1.AdjointMap().transpose(), rhs_weq, + gtsam::noiseModel::Constrained::All(6)); + + // wrench planar factor + if (planar_axis) { + gtsam::Matrix36 J_wrench = getPlanarJacobian(*planar_axis); + graph.add(internal::WrenchKey(child()->id(), id(), t), J_wrench, + gtsam::Vector3::Zero(), gtsam::noiseModel::Constrained::All(3)); + } + + return graph; +} + +/* ************************************************************************* */ +Vector6 Joint::childTwist(double q_dot) const { return cScrewAxis_ * q_dot; } + +/* ************************************************************************* */ +Vector6 Joint::parentTwist(double q_dot) const { return pScrewAxis_ * q_dot; } + +/* ************************************************************************* */ +gtsam::NonlinearFactorGraph Joint::jointLimitFactors( + size_t t, const OptimizerSetting &opt) const { + gtsam::NonlinearFactorGraph graph; + auto id = this->id(); + // Add joint angle limit factor. + graph.emplace_shared( + internal::JointAngleKey(id, t), opt.jl_cost_model, + parameters().scalar_limits.value_lower_limit, + parameters().scalar_limits.value_upper_limit, + parameters().scalar_limits.value_limit_threshold); + + // Add joint velocity limit factors. + graph.emplace_shared( + internal::JointVelKey(id, t), opt.jl_cost_model, + -parameters().velocity_limit, parameters().velocity_limit, + parameters().velocity_limit_threshold); + + // Add joint acceleration limit factors. + graph.emplace_shared( + internal::JointAccelKey(id, t), opt.jl_cost_model, + -parameters().acceleration_limit, parameters().acceleration_limit, + parameters().acceleration_limit_threshold); + + // Add joint torque limit factors. + graph.emplace_shared( + internal::TorqueKey(id, t), opt.jl_cost_model, -parameters().torque_limit, + parameters().torque_limit, parameters().torque_limit_threshold); + return graph; +} + +/* ************************************************************************* */ std::ostream &Joint::to_stream(std::ostream &os) const { os << name_ << "\n\tid=" << size_t(id_) << "\n\tparent link: " << parent()->name() - << "\n\tchild link: " << child()->name(); + << "\n\tchild link: " << child()->name() + << "\n\tscrew axis (parent): " << screwAxis(parent()).transpose(); return os; } diff --git a/gtdynamics/universal_robot/Joint.h b/gtdynamics/universal_robot/Joint.h index cc5ca67b..f3fe46c8 100644 --- a/gtdynamics/universal_robot/Joint.h +++ b/gtdynamics/universal_robot/Joint.h @@ -41,6 +41,13 @@ class Link; // forward declaration LINK_TYPEDEF_CLASS_POINTER(Link); LINK_TYPEDEF_CLASS_POINTER(Joint); +/** + * Joint effort types + * + * Actuated: motor powered + * Unactuated: not powered, free to move, exert zero torque + * Impedance: with spring resistance + */ enum JointEffortType { Actuated, Unactuated, Impedance }; /** @@ -81,21 +88,14 @@ class Joint : public boost::enable_shared_from_this { protected: using Pose3 = gtsam::Pose3; + using Vector6 = gtsam::Vector6; public: - /** - * Joint effort types - * - * Actuated: motor powered - * Unactuated: not powered, free to move, exert zero torque - * Impedance: with spring resistance - */ - + /// Joint Type (see Lee09mmt, 2.1 paragraph 2) enum Type : char { Revolute = 'R', Prismatic = 'P', - Screw = 'C', - ScrewAxis = 'A' + Screw = 'H', }; protected: @@ -114,6 +114,10 @@ class Joint : public boost::enable_shared_from_this { LinkSharedPtr parent_link_; LinkSharedPtr child_link_; + // Screw axis in parent and child CoM frames. + Vector6 pScrewAxis_; + Vector6 cScrewAxis_; + /// Joint parameters struct. JointParams parameters_; @@ -132,10 +136,12 @@ class Joint : public boost::enable_shared_from_this { * @param[in] bTj joint pose expressed in base frame * @param[in] parent_link Shared pointer to the parent Link. * @param[in] child_link Shared pointer to the child Link. + * @param[in] jScrewAxis Screw axis in the joint frame * @param[in] parameters The joint parameters. */ Joint(uint8_t id, const std::string &name, const Pose3 &bTj, const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link, + const Vector6 &jScrewAxis, const JointParams ¶meters = JointParams()); /** @@ -158,6 +164,11 @@ class Joint : public boost::enable_shared_from_this { /// Transform from the joint frame to the child's center of mass. const Pose3 &jMc() const { return jMc_; } + /// Return screw axis expressed in the specified link frame + const Vector6 screwAxis(const LinkSharedPtr &link) const { + return isChildLink(link) ? cScrewAxis_ : pScrewAxis_; + } + /// Get a gtsam::Key for this joint gtsam::Key key() const { return gtsam::Key(id()); } @@ -214,80 +225,78 @@ class Joint : public boost::enable_shared_from_this { */ virtual Type type() const = 0; + /**@}*/ + /** - * Abstract method. Return the pose of the child link CoM in the parent link - * CoM frame, given a Values object containing the joint coordinate. + * Return transform of child link CoM frame w.r.t parent link CoM frame */ - virtual Pose3 parentTchild( - const gtsam::Values &q, size_t t = 0, - boost::optional H_q = boost::none) const = 0; + Pose3 parentTchild(double q, + gtsam::OptionalJacobian<6, 1> pMc_H_q = boost::none) const; /** - * Abstract method. Return the pose of the parent link in the child link - * frame, given a Values object containing the joint coordinate. + * Return transform of parent link CoM frame w.r.t child link CoM frame */ - virtual Pose3 childTparent( - const gtsam::Values &q, size_t t = 0, - boost::optional H_q = boost::none) const = 0; + Pose3 childTparent(double q, + gtsam::OptionalJacobian<6, 1> cMp_H_q = boost::none) const; /** - * Abstract method. Return the relative pose of the specified link [link2] in - * the other link's [link1] reference frame. + * Return the relative pose of the specified link [link2] in the other link's + * [link1] reference frame. */ - virtual Pose3 relativePoseOf( - const LinkSharedPtr &link2, const gtsam::Values &q, size_t t = 0, - boost::optional H_q = boost::none) const = 0; + Pose3 relativePoseOf(const LinkSharedPtr &link2, double q, + gtsam::OptionalJacobian<6, 1> H_q = boost::none) const { + return isChildLink(link2) ? parentTchild(q, H_q) : childTparent(q, H_q); + } /** * Return the world pose of the specified link [link2], given * the world pose of the other link [link1]. */ - Pose3 poseOf(const LinkSharedPtr &link2, const Pose3 &wT1, - const gtsam::Values &q, size_t t = 0, + Pose3 poseOf(const LinkSharedPtr &link2, const Pose3 &wT1, double q, gtsam::OptionalJacobian<6, 6> H_wT1 = boost::none, - boost::optional H_q = boost::none) const { - auto T12 = relativePoseOf(link2, q, t, H_q); + gtsam::OptionalJacobian<6, 1> H_q = boost::none) const { + auto T12 = relativePoseOf(link2, q, H_q); return wT1.compose(T12, H_wT1); // H_wT2_T12 is identity } /** Abstract method. Return the twist of the other link given this link's * twist and a Values object containing this joint's angle Value. */ - virtual gtsam::Vector6 transformTwistTo( - size_t t, const LinkSharedPtr &link, const gtsam::Values &q_and_q_dot, + gtsam::Vector6 transformTwistTo( + const LinkSharedPtr &link, double q, double q_dot, boost::optional other_twist = boost::none, - boost::optional H_q = boost::none, - boost::optional H_q_dot = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const = 0; + gtsam::OptionalJacobian<6, 1> H_q = boost::none, + gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, + gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const; /** * Return the twist acceleration of the other link given this link's twist * accel and a Values object containing this joint's angle Value and * derivatives. */ - virtual gtsam::Vector6 transformTwistAccelTo( - size_t t, const LinkSharedPtr &link, - const gtsam::Values &q_and_q_dot_and_q_ddot, + gtsam::Vector6 transformTwistAccelTo( + const LinkSharedPtr &link, double q, double q_dot, double q_ddot, boost::optional this_twist = boost::none, boost::optional other_twist_accel = boost::none, - boost::optional H_q = boost::none, - boost::optional H_q_dot = boost::none, - boost::optional H_q_ddot = boost::none, + gtsam::OptionalJacobian<6, 1> H_q = boost::none, + gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, + gtsam::OptionalJacobian<6, 1> H_q_ddot = boost::none, gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist_accel = - boost::none) const = 0; - - /// Abstract method. Returns forward dynamics priors on torque - virtual gtsam::GaussianFactorGraph linearFDPriors( - size_t t, const gtsam::Values &torques, - const OptimizerSetting &opt) const { - throw std::runtime_error( - "linearFDPriors not implemented for the desired " - "joint type. A linearized version may not be possible."); - } + gtsam::OptionalJacobian<6, 6> H_other_twist_accel = boost::none) const; + + /// Return the torque on this joint given the wrench + double transformWrenchToTorque( + const LinkSharedPtr &link, + boost::optional wrench = boost::none, + gtsam::OptionalJacobian<1, 6> H_wrench = boost::none) const; + + /// Returns forward dynamics priors on torque + gtsam::GaussianFactorGraph linearFDPriors(size_t t, + const gtsam::Values &torques, + const OptimizerSetting &opt) const; /** - * @fn (ABSTRACT) Return linear accel factors in the dynamics graph. + * Returns linear acceleration factors in the dynamics graph. * * @param[in] t The timestep for which to generate factors. * @param[in] known_values Link poses, twists, Joint angles, Joint velocities. @@ -295,16 +304,12 @@ class Joint : public boost::enable_shared_from_this { * @param[in] planar_axis Optional planar axis. * @return linear accel factors. */ - virtual gtsam::GaussianFactorGraph linearAFactors( + gtsam::GaussianFactorGraph linearAFactors( size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis = boost::none) const { - throw std::runtime_error( - "linearAFactors not implemented for the desired " - "joint type. A linearized version may not be possible."); - } + const boost::optional &planar_axis = boost::none) const; /** - * @fn (ABSTRACT) Return linear dynamics factors in the dynamics graph. + * Returns linear dynamics factors in the dynamics graph. * * @param[in] t The timestep for which to generate factors. * @param[in] known_values Link poses, twists, Joint angles, Joint @@ -314,83 +319,51 @@ class Joint : public boost::enable_shared_from_this { * @param[in] planar_axis Optional planar axis. * @return linear dynamics factors. */ - virtual gtsam::GaussianFactorGraph linearDynamicsFactors( + gtsam::GaussianFactorGraph linearDynamicsFactors( size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis = boost::none) const { - throw std::runtime_error( - "linearDynamicsFactors not implemented for the " - "desired joint type. A linearized version may not be possible."); - } - - /** - * @fn (ABSTRACT) Return linear dynamics factors in the dynamics graph. - * - * @param[in] t The timestep for which to generate factors. - * @param[in] poses Link poses. - * @param[in] twists Link twists. - * @param[in] joint_angles Joint angles. - * @param[in] joint_vels Joint velocities. - * @param[in] opt OptimizerSetting object containing NoiseModels - * for factors. - * @param[in] planar_axis Optional planar axis. - * @return linear dynamics factors. - */ - virtual gtsam::GaussianFactorGraph linearDynamicsFactors( - size_t t, const std::map &poses, - const std::map &twists, - const std::map &joint_angles, - const std::map &joint_vels, - const OptimizerSetting &opt, - const boost::optional &planar_axis = boost::none) const { - throw std::runtime_error( - "linearDynamicsFactors not implemented for the " - "desired joint type. A linearized version may not be possible."); - } + const boost::optional &planar_axis = boost::none) const; /** - * @fn (ABSTRACT) Return joint limit factors in the dynamics graph. + * Return joint limit factors in the dynamics graph. * * @param[in] t The timestep for which to generate joint limit factors. * @param[in] opt OptimizerSetting object containing NoiseModels for factors. * @return joint limit factors. + * + * TODO(gerry): remove this out of Joint and into DynamicsGraph */ - virtual gtsam::NonlinearFactorGraph jointLimitFactors( - size_t t, const OptimizerSetting &opt) const = 0; - - /**@}*/ + gtsam::NonlinearFactorGraph jointLimitFactors( + size_t t, const OptimizerSetting &opt) const; /// Joint-induced twist in child frame - virtual gtsam::Vector6 childTwist(const gtsam::Values &values, - size_t t = 0) const = 0; + Vector6 childTwist(double q_dot) const; /// Joint-induced twist in parent frame - virtual gtsam::Vector6 parentTwist(const gtsam::Values &values, - size_t t = 0) const = 0; + Vector6 parentTwist(double q_dot) const; /// Calculate pose/twist of child given parent pose/twist std::pair childPoseTwist( const gtsam::Pose3 &wTp, const gtsam::Vector6 &Vp, - const gtsam::Values &known_values, size_t t = 0) const { - const gtsam::Pose3 pTc = parentTchild(known_values, t); - return {wTp * pTc, pTc.inverse().Adjoint(Vp) + childTwist(known_values, t)}; + double q, double q_dot) const { + const gtsam::Pose3 pTc = parentTchild(q); + return {wTp * pTc, pTc.inverse().Adjoint(Vp) + childTwist(q_dot)}; } /// Calculate pose/twist of parent given child pose/twist std::pair parentPoseTwist( - const gtsam::Pose3 &wTc, const gtsam::Vector6 &Vc, - const gtsam::Values &known_values, size_t t = 0) const { - const gtsam::Pose3 pTc = parentTchild(known_values, t); + const gtsam::Pose3 &wTc, const gtsam::Vector6 &Vc, double q, + double q_dot) const { + const gtsam::Pose3 pTc = parentTchild(q); return {wTc * pTc.inverse(), - pTc.Adjoint(Vc) + parentTwist(known_values, t)}; + pTc.Adjoint(Vc) + parentTwist(q_dot)}; } /// Given link pose/twist, calculate pose/twist of other link std::pair otherPoseTwist( const LinkSharedPtr &link, const gtsam::Pose3 &wTl, - const gtsam::Vector6 &Vl, const gtsam::Values &known_values, - size_t t = 0) const { - return isChildLink(link) ? parentPoseTwist(wTl, Vl, known_values, t) - : childPoseTwist(wTl, Vl, known_values, t); + const gtsam::Vector6 &Vl, double q, double q_dot) const { + return isChildLink(link) ? parentPoseTwist(wTl, Vl, q, q_dot) + : childPoseTwist(wTl, Vl, q, q_dot); } }; diff --git a/gtdynamics/universal_robot/JointTyped.h b/gtdynamics/universal_robot/JointTyped.h deleted file mode 100644 index 46dcef73..00000000 --- a/gtdynamics/universal_robot/JointTyped.h +++ /dev/null @@ -1,243 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file JointTyped.h - * @brief Specialized form of Joint for specific joint angle types - * @author: Frank Dellaert - * @author: Mandy Xie - * @author: Alejandro Escontrela - * @author: Yetong Zhang - * @author: Varun Agrawal - */ - -#pragma once - -#include "gtdynamics/universal_robot/Joint.h" -#include "gtdynamics/utils/values.h" - -namespace gtdynamics { - -/** - * JointTyped is a convenience class that inherits from Joint which wraps - * abstract transformXXX that take in joint type arguments into transformXXX - * from Joint which take in gtsam::Values object - */ - -// TODO(Gerry) JointTyped was an intermediate step towards adding ball and -// sphere joints but we never finished it because for other joint types, -// relativePoseOf can't just use a double as the joint angle -// argument, they need Unit3 or Rot3 - -class JointTyped : public Joint { -public: - using This = JointTyped; - - using JointCoordinateType = double; // standin for template - using JointCoordinateTangentType = double; // standin for template - using JointCoordinate = JointCoordinateType; - using JointVelocity = JointCoordinateTangentType; - using JointAcceleration = JointCoordinateTangentType; - using JointTorque = JointCoordinateTangentType; - - enum { N = gtsam::traits::dimension }; - using VectorN = Eigen::Matrix; - using MatrixN = Eigen::Matrix; - -public: - /// Inherit constructors - using Joint::Joint; - - /** - * @name Abstract - * Joint classes must implement these methods. - */ - ///@{ - - /** - * Abstract method. Return the pose of the child link CoM in the parent link - * CoM frame, given the joint coordinate. - */ - virtual Pose3 parentTchild( - JointCoordinate q, - gtsam::OptionalJacobian<6, N> H_q = boost::none) const = 0; - - /** - * Abstract method. Return the pose of the parent link in the child link - * frame, given the joint coordinate. - */ - virtual Pose3 - childTparent(JointCoordinate q, - gtsam::OptionalJacobian<6, N> H_q = boost::none) const = 0; - - /// Joint-induced twist in child frame - virtual gtsam::Vector6 childTwist(JointVelocity q_dot) const = 0; - - /// Joint-induced twist in parent frame - virtual gtsam::Vector6 parentTwist(JointVelocity q_dot) const = 0; - - /** - * Abstract method. Return the twist of this link given the other link's twist - * and joint angle. - */ - virtual gtsam::Vector6 transformTwistTo( - const LinkSharedPtr &link, JointCoordinate q, JointVelocity q_dot, - boost::optional other_twist = boost::none, - gtsam::OptionalJacobian<6, N> H_q = boost::none, - gtsam::OptionalJacobian<6, N> H_q_dot = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const = 0; - - /** Abstract method. Return the twist acceleration of this link given the - * other link's twist acceleration, both links' twists, and joint angle. - */ - virtual gtsam::Vector6 transformTwistAccelTo( - const LinkSharedPtr &link, JointCoordinate q, JointVelocity q_dot, - JointAcceleration q_ddot, - boost::optional this_twist = boost::none, - boost::optional other_twist_accel = boost::none, - gtsam::OptionalJacobian<6, N> H_q = boost::none, - gtsam::OptionalJacobian<6, N> H_q_dot = boost::none, - gtsam::OptionalJacobian<6, N> H_q_ddot = boost::none, - gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist_accel = - boost::none) const = 0; - - /// Abstract method. Return the torque on this joint given the wrench - virtual JointTorque transformWrenchToTorque( - const LinkSharedPtr &link, - boost::optional wrench = boost::none, - gtsam::OptionalJacobian H_wrench = boost::none) const = 0; - - /// Calculate AdjointMap jacobian w.r.t. joint coordinate q. - /// TODO(gerry + stephanie): change to calculate the jacobian of Ad_T(v) wrt T - /// rather than jacobian of Ad_T wrt q (and put in utils or PR to GTSAM) - virtual gtsam::Matrix6 - AdjointMapJacobianJointAngle(const LinkSharedPtr &link, - JointCoordinate q) const = 0; - - ///@} - /** - * @name generic - * These are methods that can be implemented here in terms of abstract methods. - */ - ///@{ - - /** - * Return the relative pose of the specified link [link2] in the other link's - * [link1] reference frame. - */ - Pose3 relativePoseOf(const LinkSharedPtr &link2, JointCoordinate q, - gtsam::OptionalJacobian<6, 1> H_q = boost::none) const { - return isChildLink(link2) ? parentTchild(q, H_q) : childTparent(q, H_q); - } - - /** - * Return the world pose of the specified link [link2], given - * the world pose of the other link [link1]. - */ - Pose3 poseOf(const LinkSharedPtr &link2, const Pose3 &wT1, JointCoordinate q, - gtsam::OptionalJacobian<6, 6> H_wT1 = boost::none, - gtsam::OptionalJacobian<6, N> H_q = boost::none) const { - auto T12 = relativePoseOf(link2, q, H_q); - return wT1.compose(T12, H_wT1); // H_wT2_T12 is identity - } - - ///@} - /** - * @name valueBased - * Methods that extract coordinates/velocities from Values and pass on to - * abstract methods above. - */ - ///@{ - - /** - * Return the pose of the child link CoM in the parent link CoM frame, given a - * Values object containing the joint coordinate. - */ - Pose3 parentTchild( - const gtsam::Values &q, size_t t = 0, - boost::optional H_q = boost::none) const override { - return parentTchild(JointAngle(q, id(), t), H_q); - } - - /** - * Return the pose of the parent link in the child link frame, given a Values - * object containing the joint coordinate. - */ - Pose3 childTparent( - const gtsam::Values &q, size_t t = 0, - boost::optional H_q = boost::none) const override { - return childTparent(JointAngle(q, id(), t), H_q); - } - - /** - * Return the relative pose of the specified link [link2] in - * the other link's [link1] reference frame. - * @throw KeyDoesNotExist if the appropriate key is missing from values - */ - Pose3 relativePoseOf( - const LinkSharedPtr &link2, const gtsam::Values &q, size_t t = 0, - boost::optional H_q = boost::none) const override { - return relativePoseOf(link2, JointAngle(q, id(), t), H_q); - } - - /// Joint-induced twist in child frame - gtsam::Vector6 childTwist(const gtsam::Values &values, - size_t t = 0) const override { - return childTwist(JointVel(values, id(), t)); - } - - /// Joint-induced twist in parent frame - gtsam::Vector6 parentTwist(const gtsam::Values &values, - size_t t = 0) const override { - return parentTwist(JointVel(values, id(), t)); - } - - /** - * Return the twist of this link given the other link's twist and a Values - * object containing this joint's angle Value. - * @param values containing q, q_dot - * @throw ValuesKeyDoesNotExist if the appropriate key is missing from values - */ - gtsam::Vector6 transformTwistTo( - size_t t, const LinkSharedPtr &link, const gtsam::Values &values, - boost::optional other_twist = boost::none, - boost::optional H_q = boost::none, - boost::optional H_q_dot = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) // - const override { - return transformTwistTo(link, JointAngle(values, id(), t), - JointVel(values, id(), t), - other_twist, H_q, H_q_dot, H_other_twist); - } - - /** Return the twist acceleration of the other link given this link's twist - * accel and a Values object containing this joint's angle value and - * derivatives. - * @param values containing q, q_dot, and q_ddot - * @throw ValuesKeyDoesNotExist if the appropriate key is missing from values - */ - gtsam::Vector6 transformTwistAccelTo( - size_t t, const LinkSharedPtr &link, const gtsam::Values &values, - boost::optional this_twist = boost::none, - boost::optional other_twist_accel = boost::none, - boost::optional H_q = boost::none, - boost::optional H_q_dot = boost::none, - boost::optional H_q_ddot = boost::none, - gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist_accel = - boost::none) const override { - return transformTwistAccelTo(link, - JointAngle(values, id(), t), - JointVel(values, id(), t), - JointAccel(values, id(), t), - this_twist, other_twist_accel, H_q, H_q_dot, - H_q_ddot, H_this_twist, H_other_twist_accel); - } -}; - -} // namespace gtdynamics diff --git a/gtdynamics/universal_robot/PrismaticJoint.h b/gtdynamics/universal_robot/PrismaticJoint.h index 091c74dc..1375e712 100644 --- a/gtdynamics/universal_robot/PrismaticJoint.h +++ b/gtdynamics/universal_robot/PrismaticJoint.h @@ -7,7 +7,7 @@ /** * @file PrismaticJoint.h - * @brief Representation of PrismaticJoint that inherits from ScrewJointBase + * @brief Representation of PrismaticJoint that inherits from Joint * @author Frank Dellaert * @author Mandy Xie * @author Alejandro Escontrela @@ -18,16 +18,16 @@ #pragma once -#include "gtdynamics/universal_robot/ScrewJointBase.h" +#include "gtdynamics/universal_robot/Joint.h" namespace gtdynamics { /** - * @class PrismaticJoint is an implementation of the ScrewJointBase class - * which represents a prismatic joint and contains all necessary factor - * construction methods. + * @class PrismaticJoint is an implementation of the Joint class which + * represents a prismatic joint and contains all necessary factor construction + * methods. */ -class PrismaticJoint : public ScrewJointBase { +class PrismaticJoint : public Joint { protected: /// Returns the screw axis in the joint frame given the joint axis gtsam::Vector6 getScrewAxis(const gtsam::Vector3 &axis) { @@ -53,11 +53,11 @@ class PrismaticJoint : public ScrewJointBase { const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link, const gtsam::Vector3 &axis, const JointParams ¶meters = JointParams()) - : ScrewJointBase(id, name, bTj, parent_link, child_link, axis, - getScrewAxis(axis), parameters) {} + : Joint(id, name, bTj, parent_link, child_link, getScrewAxis(axis), + parameters) {} /// Return joint type for use in reconstructing robot from JointParams. - Type type() const override { return Type::Prismatic; } + Type type() const final override { return Type::Prismatic; } }; } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/RevoluteJoint.h b/gtdynamics/universal_robot/RevoluteJoint.h index d0be1aaa..1675bcea 100644 --- a/gtdynamics/universal_robot/RevoluteJoint.h +++ b/gtdynamics/universal_robot/RevoluteJoint.h @@ -18,16 +18,15 @@ #pragma once -#include "gtdynamics/universal_robot/ScrewJointBase.h" +#include "gtdynamics/universal_robot/Joint.h" namespace gtdynamics { /** - * @class RevoluteJoint is an implementation of the ScrewJointBase class - * which represents a revolute joint and contains all necessary factor - * construction methods. + * @class RevoluteJoint is an implementation of the Joint class which represents + * a revolute joint and contains all necessary factor construction methods. */ -class RevoluteJoint : public ScrewJointBase { +class RevoluteJoint : public Joint { protected: /// Returns the screw axis in the joint frame given the joint axis gtsam::Vector6 getScrewAxis(const gtsam::Vector3 &axis) { @@ -53,11 +52,11 @@ class RevoluteJoint : public ScrewJointBase { const LinkSharedPtr &parent_link, const LinkSharedPtr &child_link, const gtsam::Vector3 &axis, const JointParams ¶meters = JointParams()) - : ScrewJointBase(id, name, bTj, parent_link, child_link, axis, - getScrewAxis(axis), parameters) {} + : Joint(id, name, bTj, parent_link, child_link, getScrewAxis(axis), + parameters) {} /// Return joint type for use in reconstructing robot from JointParams. - Type type() const override { return Type::Revolute; } + Type type() const final override { return Type::Revolute; } }; } // namespace gtdynamics diff --git a/gtdynamics/universal_robot/Robot.cpp b/gtdynamics/universal_robot/Robot.cpp index a3aa2cc7..635f7fb5 100644 --- a/gtdynamics/universal_robot/Robot.cpp +++ b/gtdynamics/universal_robot/Robot.cpp @@ -21,7 +21,6 @@ #include "gtdynamics/universal_robot/Joint.h" #include "gtdynamics/universal_robot/RobotTypes.h" -#include "gtdynamics/universal_robot/ScrewJointBase.h" #include "gtdynamics/utils/utils.h" #include "gtdynamics/utils/values.h" @@ -145,10 +144,7 @@ void Robot::print(const std::string &s) const { for (const auto &joint : sorted_joints) { cout << joint << endl; - gtsam::Values joint_angles; - InsertJointAngle(&joint_angles, joint->id(), 0.0); - - auto pTc = joint->parentTchild(joint_angles); + auto pTc = joint->parentTchild(0.0); cout << "\tpMc: " << pTc.rotation().rpy().transpose() << ", " << pTc.translation().transpose() << "\n"; } @@ -259,7 +255,9 @@ gtsam::Values Robot::forwardKinematics( // Loop through all joints to find the pose and twist of child links. for (auto &&joint : link1->joints()) { InsertZeroDefaults(joint->id(), t, &values); - const auto poseTwist = joint->otherPoseTwist(link1, T_w1, V_1, values, t); + const auto poseTwist = joint->otherPoseTwist( + link1, T_w1, V_1, JointAngle(values, joint->id(), t), + JointVel(values, joint->id(), t)); const auto link2 = joint->otherLink(link1); if (InsertWithCheck(link2->id(), t, poseTwist, &values)) { q.push(link2); diff --git a/gtdynamics/universal_robot/ScrewJointBase.cpp b/gtdynamics/universal_robot/ScrewJointBase.cpp deleted file mode 100644 index ab43f658..00000000 --- a/gtdynamics/universal_robot/ScrewJointBase.cpp +++ /dev/null @@ -1,265 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file ScrewJointBase.cpp - * @brief Representation of screw-type robot joints. Revolute, Prismatic, and - * Screw subclasses - * @author Frank Dellaert - * @author Mandy Xie - * @author Alejandro Escontrela - * @author Yetong Zhang - * @author Stephanie McCormick - * @author Gerry Chen - * @author Varun Agrawal - */ - - -#include "gtdynamics/universal_robot/ScrewJointBase.h" - -#include "gtdynamics/factors/JointLimitFactor.h" -#include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/utils/utils.h" -#include "gtdynamics/utils/values.h" - -#include - -#include -#include -#include - -using namespace gtsam; - -namespace gtdynamics { - -/* ************************************************************************* */ -Pose3 ScrewJointBase::parentTchild( - double q, gtsam::OptionalJacobian<6, 1> pMc_H_q) const { - // Calculate pose of child in parent link, at rest. - // TODO(dellaert): store `pMj_` rather than `jMp_`. - // NOTE(dellaert): Only if this is called more often than childTparent. - const Pose3 pMc = jMp_.inverse() * jMc_; - - // Multiply screw axis with joint angle to get a finite 6D screw. - const Vector6 screw = cScrewAxis_ * q; - - // Calculate the actual relative pose taking into account the joint angle. - // TODO(dellaert): use formula `pMj_ * screw_around_Z * jMc_`. - gtsam::Matrix6 exp_H_screw; - const Pose3 exp = Pose3::Expmap(screw, pMc_H_q ? &exp_H_screw : 0); - if (pMc_H_q) { - *pMc_H_q = exp_H_screw * cScrewAxis_; - } - return pMc * exp; // Note: derivative of compose in exp is identity. -} - -/* ************************************************************************* */ -Pose3 ScrewJointBase::childTparent( - double q, gtsam::OptionalJacobian<6, 1> cMp_H_q) const { - // TODO(frank): don't go via inverse, specialize in base class - Vector6 pMc_H_q; - Pose3 pMc = parentTchild(q, cMp_H_q ? &pMc_H_q : 0); // pMc(q) -> pMc_H_q - gtsam::Matrix6 cMp_H_pMc; - Pose3 cMp = pMc.inverse(cMp_H_q ? &cMp_H_pMc : 0); // cMp(pMc) -> cMp_H_pMc - if (cMp_H_q) { - *cMp_H_q = cMp_H_pMc * pMc_H_q; - } - return cMp; -} - -/* ************************************************************************* */ -Vector6 ScrewJointBase::transformTwistTo( - const LinkSharedPtr &link, double q, double q_dot, - boost::optional other_twist, - gtsam::OptionalJacobian<6, 1> H_q, - gtsam::OptionalJacobian<6, 1> H_q_dot, - gtsam::OptionalJacobian<6, 6> H_other_twist) const { - Vector6 other_twist_ = other_twist ? *other_twist : Vector6::Zero(); - - auto other = otherLink(link); - auto this_ad_other = relativePoseOf(other, q).AdjointMap(); - - if (H_q) { - // TODO(frank): really, zero below? Check derivatives - *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screwAxis(link)) * - other_twist_; - } - if (H_q_dot) { - *H_q_dot = screwAxis(link); - } - if (H_other_twist) { - *H_other_twist = this_ad_other; - } - - return this_ad_other * other_twist_ + screwAxis(link) * q_dot; -} - -/* ************************************************************************* */ -Vector6 ScrewJointBase::transformTwistAccelTo( - const LinkSharedPtr &link, double q, double q_dot, double q_ddot, - boost::optional this_twist, - boost::optional other_twist_accel, - gtsam::OptionalJacobian<6, 1> H_q, - gtsam::OptionalJacobian<6, 1> H_q_dot, - gtsam::OptionalJacobian<6, 1> H_q_ddot, - gtsam::OptionalJacobian<6, 6> H_this_twist, - gtsam::OptionalJacobian<6, 6> H_other_twist_accel) const { - Vector6 this_twist_ = this_twist ? *this_twist : Vector6::Zero(); - Vector6 other_twist_accel_ = - other_twist_accel ? *other_twist_accel : Vector6::Zero(); - Vector6 screw_axis_ = isChildLink(link) ? cScrewAxis_ : pScrewAxis_; - - // i = other link - // j = this link - auto other = otherLink(link); - Pose3 jTi = relativePoseOf(other, q); - - Vector6 this_twist_accel = - jTi.AdjointMap() * other_twist_accel_ + - Pose3::adjoint(this_twist_, screw_axis_ * q_dot, H_this_twist) + - screw_axis_ * q_ddot; - - if (H_other_twist_accel) { - *H_other_twist_accel = jTi.AdjointMap(); - } - if (H_q) { - // TODO(frank): really, zero below? Check derivatives. Also, copy/pasta from - // above? - *H_q = AdjointMapJacobianQ(q, relativePoseOf(other, 0.0), screw_axis_) * - other_twist_accel_; - } - if (H_q_dot) { - *H_q_dot = Pose3::adjointMap(this_twist_) * screw_axis_; - } - if (H_q_ddot) { - *H_q_ddot = screw_axis_; - } - - return this_twist_accel; -} - -/* ************************************************************************* */ -ScrewJointBase::JointTorque ScrewJointBase::transformWrenchToTorque( - const LinkSharedPtr &link, boost::optional wrench, - gtsam::OptionalJacobian<1, 6> H_wrench) const { - auto screw_axis_ = screwAxis(link); - if (H_wrench) { - *H_wrench = screw_axis_.transpose(); - } - return screw_axis_.transpose() * (wrench ? *wrench : Vector6::Zero()); -} - -/* ************************************************************************* */ -gtsam::GaussianFactorGraph ScrewJointBase::linearFDPriors( - size_t t, const gtsam::Values &known_values, - const OptimizerSetting &opt) const { - gtsam::GaussianFactorGraph priors; - gtsam::Vector1 rhs(Torque(known_values, id(), t)); - // TODO(alej`andro): use optimizer settings - priors.add(internal::TorqueKey(id(), t), gtsam::I_1x1, rhs, - gtsam::noiseModel::Constrained::All(1)); - return priors; -} - -/* ************************************************************************* */ -gtsam::GaussianFactorGraph ScrewJointBase::linearAFactors( - size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis) const { - gtsam::GaussianFactorGraph graph; - - const Pose3 T_wi1 = Pose(known_values, parent()->id(), t); - const Pose3 T_wi2 = Pose(known_values, child()->id(), t); - const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1; - const Vector6 V_i2 = Twist(known_values, child()->id(), t); - const Vector6 S_i2_j = screwAxis(child_link_); - const double v_j = JointVel(known_values, id(), t); - - // twist acceleration factor - // A_i2 - Ad(T_21) * A_i1 - S_i2_j * a_j = ad(V_i2) * S_i2_j * v_j - Vector6 rhs_tw = Pose3::adjointMap(V_i2) * S_i2_j * v_j; - graph.add(internal::TwistAccelKey(child()->id(), t), gtsam::I_6x6, - internal::TwistAccelKey(parent()->id(), t), -T_i2i1.AdjointMap(), - internal::JointAccelKey(id(), t), -S_i2_j, rhs_tw, - gtsam::noiseModel::Constrained::All(6)); - - return graph; -} - -/* ************************************************************************* */ -gtsam::GaussianFactorGraph ScrewJointBase::linearDynamicsFactors( - size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis) const { - gtsam::GaussianFactorGraph graph; - - const Pose3 T_wi1 = Pose(known_values, parent()->id(), t); - const Pose3 T_wi2 = Pose(known_values, child()->id(), t); - const Pose3 T_i2i1 = T_wi2.inverse() * T_wi1; - const Vector6 S_i2_j = screwAxis(child_link_); - - // torque factor - // S_i_j^T * F_i_j - tau = 0 - gtsam::Vector1 rhs_torque = gtsam::Vector1::Zero(); - graph.add(internal::WrenchKey(child()->id(), id(), t), S_i2_j.transpose(), - internal::TorqueKey(id(), t), -gtsam::I_1x1, rhs_torque, - gtsam::noiseModel::Constrained::All(1)); - - // wrench equivalence factor - // F_i1_j + Ad(T_i2i1)^T F_i2_j = 0 - Vector6 rhs_weq = Vector6::Zero(); - graph.add(internal::WrenchKey(parent()->id(), id(), t), gtsam::I_6x6, - internal::WrenchKey(child()->id(), id(), t), - T_i2i1.AdjointMap().transpose(), rhs_weq, - gtsam::noiseModel::Constrained::All(6)); - - // wrench planar factor - if (planar_axis) { - gtsam::Matrix36 J_wrench = getPlanarJacobian(*planar_axis); - graph.add(internal::WrenchKey(child()->id(), id(), t), J_wrench, - gtsam::Vector3::Zero(), gtsam::noiseModel::Constrained::All(3)); - } - - return graph; -} - -/* ************************************************************************* */ -gtsam::NonlinearFactorGraph ScrewJointBase::jointLimitFactors( - size_t t, const OptimizerSetting &opt) const { - gtsam::NonlinearFactorGraph graph; - auto id = this->id(); - // Add joint angle limit factor. - graph.emplace_shared( - internal::JointAngleKey(id, t), opt.jl_cost_model, - parameters().scalar_limits.value_lower_limit, - parameters().scalar_limits.value_upper_limit, - parameters().scalar_limits.value_limit_threshold); - - // Add joint velocity limit factors. - graph.emplace_shared( - internal::JointVelKey(id, t), opt.jl_cost_model, - -parameters().velocity_limit, parameters().velocity_limit, - parameters().velocity_limit_threshold); - - // Add joint acceleration limit factors. - graph.emplace_shared( - internal::JointAccelKey(id, t), opt.jl_cost_model, - -parameters().acceleration_limit, parameters().acceleration_limit, - parameters().acceleration_limit_threshold); - - // Add joint torque limit factors. - graph.emplace_shared( - internal::TorqueKey(id, t), opt.jl_cost_model, -parameters().torque_limit, - parameters().torque_limit, parameters().torque_limit_threshold); - return graph; -} - -std::ostream &ScrewJointBase::to_stream(std::ostream &os) const { - Joint::to_stream(os); - os << "\n\tscrew axis (parent): " << screwAxis(parent()).transpose(); - return os; -} - -} // namespace gtdynamics diff --git a/gtdynamics/universal_robot/ScrewJointBase.h b/gtdynamics/universal_robot/ScrewJointBase.h deleted file mode 100644 index d7e7501a..00000000 --- a/gtdynamics/universal_robot/ScrewJointBase.h +++ /dev/null @@ -1,166 +0,0 @@ -/* ---------------------------------------------------------------------------- - * GTDynamics Copyright 2020, Georgia Tech Research Corporation, - * Atlanta, Georgia 30332-0415 - * All Rights Reserved - * See LICENSE for the license information - * -------------------------------------------------------------------------- */ - -/** - * @file ScrewJointBase.h - * @brief Representation of screw-type robot joints. Revolute, Prismatic, and - * Screw subclasses - * @author Frank Dellaert - * @author Mandy Xie - * @author Alejandro Escontrela - * @author Yetong Zhang - * @author Stephanie McCormick - * @author Gerry Chen - * @author Varun Agrawal - */ - -#pragma once - -#include - -#include -#include -#include - -#include "gtdynamics/factors/JointLimitFactor.h" -#include "gtdynamics/universal_robot/JointTyped.h" -#include "gtdynamics/utils/utils.h" -#include "gtdynamics/utils/values.h" - -namespace gtdynamics { -/** - * @class ScrewJointBase is an implementation of the abstract Joint class - * which represents a screw-type joint and contains all necessary factor - * construction methods. - * It is the base class for RevoluteJoint, PrismaticJoint, and ScrewJoint. - */ -class ScrewJointBase : public JointTyped { - using Pose3 = gtsam::Pose3; - using Vector6 = gtsam::Vector6; - - protected: - gtsam::Vector3 axis_; - - // Screw axis in parent and child CoM frames. - Vector6 pScrewAxis_; - Vector6 cScrewAxis_; - - public: - /// Return transform of child link CoM frame w.r.t parent link CoM frame - Pose3 parentTchild(double q, gtsam::OptionalJacobian<6, 1> pMc_H_q = - boost::none) const override; - - protected: - /// Return transform of parent link CoM frame w.r.t child link CoM frame - Pose3 childTparent(double q, gtsam::OptionalJacobian<6, 1> cMp_H_q = - boost::none) const override; - - /** - * Return the joint axis in the joint frame. Rotational axis for revolute and - * translation direction for prismatic in the joint frame. - */ - const gtsam::Vector3 &axis() const { return axis_; } - - public: - /** - * Constructor using JointParams, joint name, bTj, screw axes, - * and parent and child links. - */ - ScrewJointBase(uint8_t id, const std::string &name, const Pose3 &bTj, - const LinkSharedPtr &parent_link, - const LinkSharedPtr &child_link, const gtsam::Vector3 &axis, - const Vector6 &jScrewAxis, - const JointParams ¶meters = JointParams()) - : JointTyped(id, name, bTj, parent_link, child_link, parameters), - axis_(axis), - pScrewAxis_(-jMp_.inverse().AdjointMap() * jScrewAxis), - cScrewAxis_(jMc_.inverse().AdjointMap() * jScrewAxis) {} - - /// Return joint type for use in reconstructing robot from JointParams. - Type type() const override { return Type::ScrewAxis; } - - /// Return screw axis expressed in the specified link frame - const Vector6 screwAxis(const LinkSharedPtr &link) const { - return isChildLink(link) ? cScrewAxis_ : pScrewAxis_; - } - - // inherit overloads - using JointTyped::poseOf; - using JointTyped::relativePoseOf; - using JointTyped::transformTwistAccelTo; - using JointTyped::transformTwistTo; - - /** - * Return the twist of this link given the other link's twist and joint angle. - */ - Vector6 transformTwistTo( - const LinkSharedPtr &link, double q, double q_dot, - boost::optional other_twist = boost::none, - gtsam::OptionalJacobian<6, 1> H_q = boost::none, - gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist = boost::none) const override; - - /** - * Return the twist acceleration of this link given the other link's twist - * acceleration, twist, and joint angle and this link's twist. - */ - Vector6 transformTwistAccelTo( - const LinkSharedPtr &link, double q, double q_dot, double q_ddot, - boost::optional this_twist = boost::none, - boost::optional other_twist_accel = boost::none, - gtsam::OptionalJacobian<6, 1> H_q = boost::none, - gtsam::OptionalJacobian<6, 1> H_q_dot = boost::none, - gtsam::OptionalJacobian<6, 1> H_q_ddot = boost::none, - gtsam::OptionalJacobian<6, 6> H_this_twist = boost::none, - gtsam::OptionalJacobian<6, 6> H_other_twist_accel = - boost::none) const override; - - JointTorque transformWrenchToTorque( - const LinkSharedPtr &link, boost::optional wrench = boost::none, - gtsam::OptionalJacobian<1, 6> H_wrench = boost::none) const override; - - // TODO(frank): document and possibly eliminate - gtsam::Matrix6 AdjointMapJacobianJointAngle(const LinkSharedPtr &link, - double q) const override { - return AdjointMapJacobianQ(q, relativePoseOf(otherLink(link), 0), - screwAxis(link)); - } - - /// Return forward dynamics priors on torque. - gtsam::GaussianFactorGraph linearFDPriors( - size_t t, const gtsam::Values &known_values, - const OptimizerSetting &opt) const override; - - /// Return linearized acceleration factors. - gtsam::GaussianFactorGraph linearAFactors( - size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis) const override; - - /// Return linearized dynamics factors. - gtsam::GaussianFactorGraph linearDynamicsFactors( - size_t t, const gtsam::Values &known_values, const OptimizerSetting &opt, - const boost::optional &planar_axis) const override; - - /// Return joint limit factors. - gtsam::NonlinearFactorGraph jointLimitFactors( - size_t t, const OptimizerSetting &opt) const override; - - /// Joint-induced twist in child frame - gtsam::Vector6 childTwist(double q_dot) const override { - return cScrewAxis_ * q_dot; - } - - /// Joint-induced twist in parent frame - gtsam::Vector6 parentTwist(double q_dot) const override { - return pScrewAxis_ * q_dot; - } - - /// Helper function for overloading stream operator - virtual std::ostream &to_stream(std::ostream &os) const override; -}; - -} // namespace gtdynamics diff --git a/gtdynamics/universal_robot/sdf.cpp b/gtdynamics/universal_robot/sdf.cpp index b164263b..f686867a 100644 --- a/gtdynamics/universal_robot/sdf.cpp +++ b/gtdynamics/universal_robot/sdf.cpp @@ -20,8 +20,7 @@ #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/universal_robot/PrismaticJoint.h" #include "gtdynamics/universal_robot/RevoluteJoint.h" -#include "gtdynamics/universal_robot/ScrewJoint.h" -#include "gtdynamics/universal_robot/ScrewJointBase.h" +#include "gtdynamics/universal_robot/HelicalJoint.h" #include "gtdynamics/universal_robot/sdf_internal.h" namespace gtdynamics { @@ -188,7 +187,7 @@ JointSharedPtr JointFromSdf(uint8_t id, const LinkSharedPtr &parent_link, child_link, axis, parameters); break; case sdf::JointType::SCREW: - joint = boost::make_shared( + joint = boost::make_shared( id, name, bMj, parent_link, child_link, axis, sdf_joint.ThreadPitch(), parameters); break; diff --git a/gtdynamics/universal_robot/sdf.h b/gtdynamics/universal_robot/sdf.h index 4d32ab7f..63eeb9b4 100644 --- a/gtdynamics/universal_robot/sdf.h +++ b/gtdynamics/universal_robot/sdf.h @@ -16,7 +16,6 @@ #include #include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/universal_robot/ScrewJointBase.h" namespace gtdynamics { diff --git a/gtdynamics/universal_robot/sdf_internal.h b/gtdynamics/universal_robot/sdf_internal.h index 4a6f9dc9..16f66c00 100644 --- a/gtdynamics/universal_robot/sdf_internal.h +++ b/gtdynamics/universal_robot/sdf_internal.h @@ -18,7 +18,6 @@ #include #include "gtdynamics/universal_robot/Robot.h" -#include "gtdynamics/universal_robot/ScrewJointBase.h" namespace gtdynamics { diff --git a/gtdynamics/utils/JsonSaver.h b/gtdynamics/utils/JsonSaver.h index 6dab8339..16e9aa83 100644 --- a/gtdynamics/utils/JsonSaver.h +++ b/gtdynamics/utils/JsonSaver.h @@ -34,7 +34,7 @@ #include "gtdynamics/factors/WrenchEquivalenceFactor.h" #include "gtdynamics/factors/WrenchFactor.h" #include "gtdynamics/factors/WrenchPlanarFactor.h" -#include "gtdynamics/universal_robot/ScrewJointBase.h" +#include "gtdynamics/universal_robot/Joint.h" #include "gtdynamics/utils/utils.h" #define kQuote_ "\"" @@ -201,9 +201,7 @@ class JsonSaver { std::stringstream ss; if (const TorqueFactor* f = dynamic_cast(&(*factor))) { auto joint = f->getJoint(); - ss << GetVector(boost::static_pointer_cast(joint) - ->screwAxis(joint->child()) - .transpose()); + ss << GetVector(joint->screwAxis(joint->child()).transpose()); } else if (const gtsam::PriorFactor* f = dynamic_cast*>( &(*factor))) { diff --git a/gtdynamics/utils/values.cpp b/gtdynamics/utils/values.cpp index 4eacb46c..3538148a 100644 --- a/gtdynamics/utils/values.cpp +++ b/gtdynamics/utils/values.cpp @@ -32,26 +32,74 @@ const char *KeyDoesNotExist::what() const noexcept { } /* ************************************************************************* */ +void InsertJointAngle(Values *values, int j, int t, double value) { + values->insert(internal::JointAngleKey(j, t), value); +} + +void InsertJointAngle(Values *values, int j, double value) { + values->insert(internal::JointAngleKey(j), value); +} + Vector JointAngle(const VectorValues &values, int j, int t) { return values.at(internal::JointAngleKey(j, t)); } +double JointAngle(const Values &values, int j, int t) { + return internal::at(values, internal::JointAngleKey(j, t)); +} + /* ************************************************************************* */ +void InsertJointVel(Values *values, int j, int t, double value) { + values->insert(internal::JointVelKey(j, t), value); +} + +void InsertJointVel(Values *values, int j, double value) { + values->insert(internal::JointVelKey(j), value); +} + Vector JointVel(const VectorValues &values, int j, int t) { return values.at(internal::JointVelKey(j, t)); } +double JointVel(const Values &values, int j, int t) { + return internal::at(values, internal::JointVelKey(j, t)); +} + /* ************************************************************************* */ +void InsertJointAccel(Values *values, int j, int t, double value) { + values->insert(internal::JointAccelKey(j, t), value); +} + +void InsertJointAccel(Values *values, int j, double value) { + values->insert(internal::JointAccelKey(j), value); +} + Vector JointAccel(const VectorValues &values, int j, int t) { return values.at(internal::JointAccelKey(j, t)); } +double JointAccel(const Values &values, int j, int t) { + return internal::at(values, internal::JointAccelKey(j, t)); +} + + /* ************************************************************************* */ -/// Retrieve torque on the j-th joint at time t. +void InsertTorque(Values *values, int j, int t, double value) { + values->insert(internal::TorqueKey(j, t), value); +} + +void InsertTorque(Values *values, int j, double value) { + values->insert(internal::TorqueKey(j), value); +} + Vector Torque(const VectorValues &values, int j, int t) { return values.at(internal::TorqueKey(j, t)); }; +double Torque(const Values &values, int j, int t) { + return internal::at(values, internal::TorqueKey(j, t)); +}; + /* ************************************************************************* */ /// Insert pose for i-th link at time t. void InsertPose(Values *values, int i, int t, Pose3 value) { diff --git a/gtdynamics/utils/values.h b/gtdynamics/utils/values.h index 35cab883..081b9588 100644 --- a/gtdynamics/utils/values.h +++ b/gtdynamics/utils/values.h @@ -98,29 +98,21 @@ T at(const gtsam::Values &values, size_t key) { /** * @brief Insert j-th joint angle at time t. * - * @tparam T Joint angle type (default `double`). * @param values Values pointer to insert joint angle into. * @param j The joint id. * @param t Time step. * @param value The joint angle value. */ -template -void InsertJointAngle(gtsam::Values *values, int j, int t, T value) { - values->insert(internal::JointAngleKey(j, t), value); -} +void InsertJointAngle(gtsam::Values *values, int j, int t, double value); /** * @brief Insert j-th joint angle at time 0. * - * @tparam T Joint angle type (default `double`). * @param values Values pointer to insert joint angle into. * @param j The joint id. * @param value The joint angle value. */ -template -void InsertJointAngle(gtsam::Values *values, int j, T value) { - values->insert(internal::JointAngleKey(j), value); -} +void InsertJointAngle(gtsam::Values *values, int j, double value); /** * @brief Retrieve j-th joint angle at time t as a Vector. @@ -136,16 +128,12 @@ gtsam::Vector JointAngle(const gtsam::VectorValues &values, int j, int t = 0); /** * @brief Retrieve j-th joint angle at time t. * - * @tparam T Joint angle type (default is `double`). * @param values Values dictionary containing the joint angle. * @param j The joint id. * @param t Time step. - * @return T + * @return The joint angle */ -template -T JointAngle(const gtsam::Values &values, int j, int t = 0) { - return internal::at(values, internal::JointAngleKey(j, t)); -} +double JointAngle(const gtsam::Values &values, int j, int t = 0); /* ************************************************************************* Functions for Joint Velocities. @@ -154,29 +142,21 @@ T JointAngle(const gtsam::Values &values, int j, int t = 0) { /** * @brief Insert j-th joint velocity at time t. * - * @tparam T Joint velocity type (default `double`). * @param values Values dictionary pointer to insert joint velocity into. * @param j The joint id. * @param t Time step. * @param value The joint velocity value. */ -template -void InsertJointVel(gtsam::Values *values, int j, int t, T value) { - values->insert(internal::JointVelKey(j, t), value); -} +void InsertJointVel(gtsam::Values *values, int j, int t, double value); /** * @brief Insert j-th joint velocity at time 0. * - * @tparam T Joint velocity type (default `double`). * @param values Values pointer to insert joint velocity into. * @param j The joint id. * @param value The joint velocity value. */ -template -void InsertJointVel(gtsam::Values *values, int j, T value) { - values->insert(internal::JointVelKey(j), value); -} +void InsertJointVel(gtsam::Values *values, int j, double value); /** * @brief Retrieve j-th joint velocity at time t. @@ -192,16 +172,12 @@ gtsam::Vector JointVel(const gtsam::VectorValues &values, int j, int t = 0); /** * @brief Retrieve j-th joint velocity at time t. * - * @tparam T Joint velocity type (default is `double`). * @param values Values dictionary containing the joint velocity. * @param j The joint id. * @param t Time step. - * @return T + * @return The joint velocity */ -template -T JointVel(const gtsam::Values &values, int j, int t = 0) { - return internal::at(values, internal::JointVelKey(j, t)); -} +double JointVel(const gtsam::Values &values, int j, int t = 0); /* ************************************************************************* Functions for Joint Accelerations. @@ -210,29 +186,21 @@ T JointVel(const gtsam::Values &values, int j, int t = 0) { /** * @brief Insert j-th joint acceleration at time t. * - * @tparam T Joint acceleration type (default `double`). * @param values Values dictionary pointer to insert joint acceleration into. * @param j The joint id. * @param t Time step. * @param value The joint acceleration value. */ -template -void InsertJointAccel(gtsam::Values *values, int j, int t, T value) { - values->insert(internal::JointAccelKey(j, t), value); -} +void InsertJointAccel(gtsam::Values *values, int j, int t, double value); /** * @brief Insert j-th joint acceleration at time 0. * - * @tparam T Joint acceleration type (default `double`). * @param values Values pointer to insert joint acceleration into. * @param j The joint id. * @param value The joint acceleration value. */ -template -void InsertJointAccel(gtsam::Values *values, int j, T value) { - values->insert(internal::JointAccelKey(j), value); -} +void InsertJointAccel(gtsam::Values *values, int j, double value); /** * @brief Retrieve j-th joint acceleration at time t. @@ -248,16 +216,12 @@ gtsam::Vector JointAccel(const gtsam::VectorValues &values, int j, int t = 0); /** * @brief Retrieve j-th joint acceleration at time t. * - * @tparam T Joint acceleration type (default is `double`). * @param values Values dictionary containing the joint acceleration. * @param j The joint id. * @param t Time step. - * @return T + * @return The joint acceleration */ -template -T JointAccel(const gtsam::Values &values, int j, int t = 0) { - return internal::at(values, internal::JointAccelKey(j, t)); -} +double JointAccel(const gtsam::Values &values, int j, int t = 0); /* ************************************************************************* Functions for Torques. @@ -266,29 +230,21 @@ T JointAccel(const gtsam::Values &values, int j, int t = 0) { /** * @brief Insert torque on the j-th joint at time t. * - * @tparam T Torque type (default `double`). * @param values Values dictionary pointer to insert torque into. * @param j The joint id. * @param t Time step. * @param value The torque value. */ -template -void InsertTorque(gtsam::Values *values, int j, int t, T value) { - values->insert(internal::TorqueKey(j, t), value); -} +void InsertTorque(gtsam::Values *values, int j, int t, double value); /** * @brief Insert torque on the j-th joint at time 0. * - * @tparam T Torque type (default `double`). * @param values Values pointer to insert torque into. * @param j The joint id. * @param value The torque value. */ -template -void InsertTorque(gtsam::Values *values, int j, T value) { - values->insert(internal::TorqueKey(j), value); -} +void InsertTorque(gtsam::Values *values, int j, double value); /** * @brief Retrieve torque on the j-th joint at time t. @@ -304,16 +260,12 @@ gtsam::Vector Torque(const gtsam::VectorValues &values, int j, int t = 0); /** * @brief Retrieve torque on the j-th joint at time t. * - * @tparam T Joint torque type (default is `double`). * @param values Values dictionary containing the joint torque. * @param j The joint id. * @param t Time step. - * @return T + * @return The joint torque */ -template -T Torque(const gtsam::Values &values, int j, int t = 0) { - return internal::at(values, internal::TorqueKey(j, t)); -}; +double Torque(const gtsam::Values &values, int j, int t = 0); /* ************************************************************************* Functions for Poses. diff --git a/python/tests/test_factors.py b/python/tests/test_factors.py index 632b08c2..338c1ca9 100644 --- a/python/tests/test_factors.py +++ b/python/tests/test_factors.py @@ -30,9 +30,7 @@ def setUp(self): def test_revolute_joint_measurement_factor(self): """Test RevoluteJointMeasurementFactor.""" - factor = gtd.RevoluteJointMeasurementFactor( - self.wTp_key, self.wTc_key, - gtsam.noiseModel.Isotropic.Sigma(6, 0.1), - self.robot.joint("joint_1"), np.pi / 4, self.k) + factor = gtd.JointMeasurementFactor(gtsam.noiseModel.Isotropic.Sigma(6, 0.1), + self.robot.joint("joint_1"), np.pi / 4, self.k) - self.assertTrue(isinstance(factor, gtd.RevoluteJointMeasurementFactor)) + self.assertTrue(isinstance(factor, gtd.JointMeasurementFactor)) diff --git a/python/tests/test_forward_kinematics.py b/python/tests/test_forward_kinematics.py index 11c85964..8950a863 100644 --- a/python/tests/test_forward_kinematics.py +++ b/python/tests/test_forward_kinematics.py @@ -45,7 +45,7 @@ def setUp(self): 11: -1.852, } for i, angle in angles.items(): - gtd.InsertJointAngleDouble(self.joint_angles, i, 0, angle) + gtd.InsertJointAngle(self.joint_angles, i, 0, angle) def test_forward_kinematics_factor(self): """Test if forward kinematics are correct.""" diff --git a/python/tests/test_four_bar.py b/python/tests/test_four_bar.py index 25b86119..371ffcfb 100644 --- a/python/tests/test_four_bar.py +++ b/python/tests/test_four_bar.py @@ -85,11 +85,11 @@ def test_four_bar(self): joint_vels = np.array([0, 0, 0, 0]) torques = np.array([1, 0, 0, 0]) for idx, joint in enumerate(robot.joints()): - gtd.InsertJointAngleDouble(known_values, joint.id(), 0, + gtd.InsertJointAngle(known_values, joint.id(), 0, joint_angles[idx]) - gtd.InsertJointVelDouble(known_values, joint.id(), 0, + gtd.InsertJointVel(known_values, joint.id(), 0, joint_vels[idx]) - gtd.InsertTorqueDouble(known_values, joint.id(), 0, torques[idx]) + gtd.InsertTorque(known_values, joint.id(), 0, torques[idx]) prior_graph = graph_builder.forwardDynamicsPriors( robot, 0, known_values) diff --git a/python/tests/test_print.py b/python/tests/test_print.py index dfd81b89..4f3b38e6 100644 --- a/python/tests/test_print.py +++ b/python/tests/test_print.py @@ -23,7 +23,7 @@ class TestPrint(unittest.TestCase): def test_values(self): """Checks that printing Values uses the GTDKeyFormatter instead of gtsam's default""" v = gtd.Values() - gtd.InsertJointAngleDouble(v, 0, 1, 2) + gtd.InsertJointAngle(v, 0, 1, 2) self.assertTrue('q(0)1' in v.__repr__()) def test_nonlinear_factor_graph(self): diff --git a/python/tests/test_robot.py b/python/tests/test_robot.py index 57642447..d39ec6ab 100644 --- a/python/tests/test_robot.py +++ b/python/tests/test_robot.py @@ -46,7 +46,7 @@ def test_forward_kinematics(self): for idx, joint in enumerate(joints): th[joint] = joint_angles[idx] - gtd.InsertJointAngleDouble(joint_angles_values, + gtd.InsertJointAngle(joint_angles_values, robot.joint(joint).id(), joint_angles[idx]) diff --git a/python/tests/test_simulator.py b/python/tests/test_simulator.py index 0f818147..5e8b97b6 100644 --- a/python/tests/test_simulator.py +++ b/python/tests/test_simulator.py @@ -39,7 +39,7 @@ def test_simulator(self): initial_values = Values() torques = Values() - gtd.InsertTorqueDouble(torques, 0, 1.0) + gtd.InsertTorque(torques, 0, 1.0) simulator = gtd.Simulator(robot, initial_values, gravity, planar_axis) @@ -52,9 +52,9 @@ def test_simulator(self): expected_qAccel = acceleration expected_qVel = acceleration * dt expected_qAngle = acceleration * 0.5 * dt * dt - self.assertEqual(expected_qAngle, gtd.JointAngleDouble(results, 0, 0)) - self.assertEqual(expected_qVel, gtd.JointVelDouble(results, 0, 0)) - self.assertEqual(expected_qAccel, gtd.JointAccelDouble(results, 0, 0)) + self.assertEqual(expected_qAngle, gtd.JointAngle(results, 0, 0)) + self.assertEqual(expected_qVel, gtd.JointVel(results, 0, 0)) + self.assertEqual(expected_qAccel, gtd.JointAccel(results, 0, 0)) if __name__ == "__main__": diff --git a/scripts/gerry00_ForwardDynamicsPrismatic.cpp b/scripts/gerry00_ForwardDynamicsPrismatic.cpp index 8f8a290d..849ff0c7 100644 --- a/scripts/gerry00_ForwardDynamicsPrismatic.cpp +++ b/scripts/gerry00_ForwardDynamicsPrismatic.cpp @@ -47,12 +47,12 @@ int main(int argc, char** argv) { int j1 = simple_rpr.joint("joint_1")->id(), j2 = simple_rpr.joint("joint_2")->id(), j3 = simple_rpr.joint("joint_3")->id(); - InsertJointAngle(&theta_and_theta_dot, j1, 0, 0.0); - InsertJointAngle(&theta_and_theta_dot, j2, 0, 0.0); - InsertJointAngle(&theta_and_theta_dot, j3, 0, 0.0); - InsertJointVel(&theta_and_theta_dot, j1, 0, 0.3); - InsertJointVel(&theta_and_theta_dot, j2, 0, 0.1); - InsertJointVel(&theta_and_theta_dot, j3, 0, 0.0); + InsertJointAngle(&theta_and_theta_dot, j1, 0, 0.0); + InsertJointAngle(&theta_and_theta_dot, j2, 0, 0.0); + InsertJointAngle(&theta_and_theta_dot, j3, 0, 0.0); + InsertJointVel(&theta_and_theta_dot, j1, 0, 0.3); + InsertJointVel(&theta_and_theta_dot, j2, 0, 0.1); + InsertJointVel(&theta_and_theta_dot, j3, 0, 0.0); std::vector taus; for (int t = 0; t <= T; t++) { diff --git a/tests/make_joint.h b/tests/make_joint.h index 1c7d2e6d..818963d7 100644 --- a/tests/make_joint.h +++ b/tests/make_joint.h @@ -14,12 +14,11 @@ #pragma once #include "gtdynamics/universal_robot/Link.h" -#include "gtdynamics/universal_robot/ScrewJointBase.h" +#include "gtdynamics/universal_robot/HelicalJoint.h" namespace gtdynamics { /// Create a joint with given rest transform cMp and screw-axis in child frame. -boost::shared_ptr make_joint(gtsam::Pose3 cMp, - gtsam::Vector6 cScrewAxis) { +JointConstSharedPtr make_joint(gtsam::Pose3 cMp, gtsam::Vector6 cScrewAxis) { // create links std::string name = "l1"; double mass = 100; @@ -41,7 +40,7 @@ boost::shared_ptr make_joint(gtsam::Pose3 cMp, gtsam::Pose3 jMc = bMj.inverse() * l2->bMcom(); gtsam::Vector6 jScrewAxis = jMc.AdjointMap() * cScrewAxis; - return boost::make_shared(ScrewJointBase( - 1, "j1", bMj, l1, l2, jScrewAxis.head<3>(), jScrewAxis, joint_params)); + return boost::make_shared( + 1, "j1", bMj, l1, l2, jScrewAxis, joint_params); } } // namespace gtdynamics diff --git a/tests/testScrewJoint.cpp b/tests/testHelicalJoint.cpp similarity index 91% rename from tests/testScrewJoint.cpp rename to tests/testHelicalJoint.cpp index dda92873..11376767 100644 --- a/tests/testScrewJoint.cpp +++ b/tests/testHelicalJoint.cpp @@ -6,8 +6,8 @@ * -------------------------------------------------------------------------- */ /** - * @file testScrewJoint.cpp - * @brief Test ScrewJoint class. + * @file testHelicalJoint.cpp + * @brief Test HelicalJoint class. * @author Frank Dellaert, Mandy Xie, Alejandro Escontrela, and Yetong Zhang */ @@ -17,7 +17,7 @@ #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/universal_robot/RobotModels.h" -#include "gtdynamics/universal_robot/ScrewJoint.h" +#include "gtdynamics/universal_robot/HelicalJoint.h" #include "gtdynamics/universal_robot/sdf.h" #include "gtdynamics/utils/utils.h" @@ -40,7 +40,7 @@ TEST(Joint, params_constructor) { parameters.scalar_limits.value_upper_limit = 1.57; parameters.scalar_limits.value_limit_threshold = 0; - auto j1 = boost::make_shared( + auto j1 = boost::make_shared( 123, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, gtsam::Vector3(1, 0, 0), 0.5, parameters); @@ -76,10 +76,6 @@ TEST(Joint, params_constructor) { EXPECT(assert_equal(T_12com, j1->relativePoseOf(l2, -M_PI / 2))); EXPECT(assert_equal(T_21com, j1->relativePoseOf(l1, -M_PI / 2))); - // should throw error - CHECK_EXCEPTION(j1->relativePoseOf(l2, gtsam::Values()), - gtsam::ValuesKeyDoesNotExist); - // links auto links = j1->links(); EXPECT(links[0] == l1); diff --git a/tests/testJointMeasurementFactor.cpp b/tests/testJointMeasurementFactor.cpp index 678b5668..62ff85a4 100644 --- a/tests/testJointMeasurementFactor.cpp +++ b/tests/testJointMeasurementFactor.cpp @@ -32,9 +32,6 @@ using namespace gtdynamics; using namespace gtsam; using gtsam::assert_equal; -const Key key0 = gtdynamics::internal::PoseKey(0), - key1 = gtdynamics::internal::PoseKey(1); - auto kModel = noiseModel::Isotropic::Sigma(6, 0.1); auto robot = simple_rr::getRobot(); @@ -42,13 +39,11 @@ size_t t = 0; // Test should not throw an exception. TEST(JointMeasurementFactor, Constructor) { - JointMeasurementFactor(key0, key1, kModel, robot.joints()[0], - 0.0, t); + JointMeasurementFactor(kModel, robot.joints()[0], 0.0, t); } TEST(JointMeasurementFactor, Error) { - JointMeasurementFactor factor(key0, key1, kModel, - robot.joints()[0], 0.0, t); + JointMeasurementFactor factor(kModel, robot.joints()[0], 0.0, t); auto link0 = robot.links()[0]; auto link1 = robot.links()[1]; @@ -58,8 +53,7 @@ TEST(JointMeasurementFactor, Error) { EXPECT(assert_equal(Vector::Zero(6), error, 1e-9)); // Error when the elbow is bent to 90 degrees - JointMeasurementFactor factor2(key0, key1, kModel, - robot.joints()[0], M_PI, t); + JointMeasurementFactor factor2(kModel, robot.joints()[0], M_PI, t); Pose3 wTl1(Rot3::Rz(M_PI), gtsam::Point3(0, 0, 0.5)); Vector error2 = factor2.evaluateError(link0->bMcom(), wTl1); @@ -68,8 +62,7 @@ TEST(JointMeasurementFactor, Error) { } TEST(JointMeasurementFactor, Jacobians) { - JointMeasurementFactor factor(key0, key1, kModel, - robot.joints()[0], 0.0, t); + JointMeasurementFactor factor(kModel, robot.joints()[0], 0.0, t); auto link0 = robot.links()[0]; auto link1 = robot.links()[1]; @@ -85,8 +78,7 @@ TEST(JointMeasurementFactor, Jacobians) { // Non-trivial joint angle double angle = M_PI; - JointMeasurementFactor factor2(key0, key1, kModel, - robot.joints()[0], angle, t); + JointMeasurementFactor factor2(kModel, robot.joints()[0], angle, t); wTl1 = Pose3(Rot3::Rz(angle), gtsam::Point3(0, 0, 0.5)); values.clear(); InsertPose(&values, link0->id(), wTl0); @@ -106,15 +98,14 @@ TEST(JointMeasurementFactor, ArbitraryTime) { // Non-trivial joint angle double angle = M_PI; - JointMeasurementFactor factor(key0, key1, kModel, - robot.joints()[0], angle, t); + JointMeasurementFactor factor(kModel, robot.joints()[0], angle, t); Pose3 wTl0 = link0->bMcom(); Pose3 wTl1 = Pose3(Rot3::Rz(angle), gtsam::Point3(0, 0, 0.5)); Values values; - InsertPose(&values, link0->id(), wTl0); - InsertPose(&values, link1->id(), wTl1); + InsertPose(&values, link0->id(), t, wTl0); + InsertPose(&values, link1->id(), t, wTl1); Vector error = factor.evaluateError(wTl0, wTl1); EXPECT(assert_equal(Vector::Zero(6), error, 1e-9)); diff --git a/tests/testPoseFactor.cpp b/tests/testPoseFactor.cpp index 3ad31ebf..607a31cf 100644 --- a/tests/testPoseFactor.cpp +++ b/tests/testPoseFactor.cpp @@ -108,8 +108,7 @@ TEST(PoseFactor, breaking_rr) { auto l1 = robot.link("l1"); auto l2 = robot.link("l2"); - auto j1 = boost::dynamic_pointer_cast( - robot.joint("j1")); + const auto& j1 = robot.joint("j1"); Vector6 screw_axis = (Vector6() << 1, 0, 0, 0, -1, 0).finished(); Pose3 cMp = j1->relativePoseOf(l1, 0.0); diff --git a/tests/testRevoluteJoint.cpp b/tests/testRevoluteJoint.cpp index 31d0717b..bd5825f3 100644 --- a/tests/testRevoluteJoint.cpp +++ b/tests/testRevoluteJoint.cpp @@ -159,15 +159,11 @@ TEST(Joint, ValuesRelativePoseOf) { RevoluteJoint j1(1, "j1", Pose3(Rot3(), Point3(0, 0, 2)), l1, l2, axis, parameters); - Values values; - const size_t t = 777; // Rotating joint by -M_PI / 2 double q = -M_PI / 2; Pose3 T12(Rot3::Rx(q), Point3(0, 1, 1)); Pose3 T21(Rot3::Rx(-q), Point3(0, 1, -1)); - InsertJointAngle(&values, 1, t, q); - // Calculate numerical derivatives of relativePoseOf with respect to other // link pose. auto f1 = [&](double q) { return j1.relativePoseOf(l1, q); }; @@ -176,8 +172,8 @@ TEST(Joint, ValuesRelativePoseOf) { Matrix61 numericalH2 = numericalDerivative11(f2, q); Matrix H1v, H2v; - EXPECT(assert_equal(T21, j1.relativePoseOf(l1, values, t, H1v))); - EXPECT(assert_equal(T12, j1.relativePoseOf(l2, values, t, H2v))); + EXPECT(assert_equal(T21, j1.relativePoseOf(l1, q, H1v))); + EXPECT(assert_equal(T12, j1.relativePoseOf(l2, q, H2v))); EXPECT(assert_equal(numericalH1, H1v)); EXPECT(assert_equal(numericalH2, H2v)); @@ -211,10 +207,7 @@ TEST(RevoluteJoint, ParentTchild) { auto robot = simple_urdf::getRobot(); auto j1 = robot.joint("j1"); - Values joint_angles; - InsertJointAngle(&joint_angles, j1->id(), M_PI_2); - - Pose3 pTc = j1->parentTchild(joint_angles); + Pose3 pTc = j1->parentTchild(M_PI_2); // Rotate around the x axis for arm point up. // This means the second link bends to the right. Pose3 expected_pTc(Rot3::Rx(M_PI_2), Point3(0, -1, 1)); diff --git a/tests/testSdf.cpp b/tests/testSdf.cpp index c26e7a67..497e7443 100644 --- a/tests/testSdf.cpp +++ b/tests/testSdf.cpp @@ -20,7 +20,7 @@ #include "gtdynamics/universal_robot/Link.h" #include "gtdynamics/universal_robot/PrismaticJoint.h" #include "gtdynamics/universal_robot/RevoluteJoint.h" -#include "gtdynamics/universal_robot/ScrewJoint.h" +#include "gtdynamics/universal_robot/HelicalJoint.h" #include "gtdynamics/universal_robot/sdf.h" #include "gtdynamics/universal_robot/sdf_internal.h" #include "gtdynamics/utils/utils.h" @@ -535,7 +535,7 @@ TEST(Sdf, sdf_constructor_screw) { j1_parameters.effort_type = JointEffortType::Actuated; const gtsam::Vector3 j1_axis = GetSdfAxis(*model.JointByName("joint_1")); - auto j1 = boost::make_shared( + auto j1 = boost::make_shared( 1, "joint_1", bTj, l0, l1, j1_axis, model.JointByName("joint_1")->ThreadPitch(), j1_parameters);