Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Kill JointTyped #291

Merged
merged 14 commits into from
Oct 27, 2021
Merged

Kill JointTyped #291

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
14 commits
Select commit Hold shift + click to select a range
bceec88
c++ compiles (but not correct due to commenting-out) for remove Joint…
gchenfc Oct 16, 2021
562b575
update python wrapper for JointTyped removal
gchenfc Oct 16, 2021
1bc9d70
update Joint::Type enum
gchenfc Oct 16, 2021
8880aaf
cleanup
gchenfc Oct 16, 2021
749532b
fix failing unit tests
gchenfc Oct 16, 2021
fe68291
kill JointTyped.h and ScrewJointBase.h files
gchenfc Oct 16, 2021
c4e5eb7
fix python unit test for RevoluteJointMeasurementFactor
gchenfc Oct 16, 2021
bb80955
values.h functions are no longer templated on JointCoordinate
gchenfc Oct 16, 2021
60122c1
update python to not use "Double" for values
gchenfc Oct 16, 2021
44d89d1
rename measured joint angle
gchenfc Oct 16, 2021
938aa69
rename ScrewJoint to HelicalJoint
gchenfc Oct 16, 2021
1432238
fix straggler <double> template
gchenfc Oct 26, 2021
7f66e4a
fix rebase mistakes
gchenfc Oct 27, 2021
d97648a
remove python deprecated functions
gchenfc Oct 27, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 4 additions & 4 deletions examples/example_inverted_pendulum_trajectory_optimization/inverted_pendulum.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down
10 changes: 5 additions & 5 deletions examples/example_jumping_robot/src/jr_simulator.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand All @@ -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)
Expand Down Expand Up @@ -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:
Expand Down Expand Up @@ -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)
Expand Down
20 changes: 10 additions & 10 deletions examples/example_jumping_robot/src/jr_values.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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

Expand All @@ -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):
Expand Down
6 changes: 3 additions & 3 deletions examples/example_jumping_robot/src/jr_visualizer.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)))
Expand Down
8 changes: 4 additions & 4 deletions examples/example_jumping_robot/tests/test_jr_simulator.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions examples/example_jumping_robot/tests/test_jumping_robot.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
77 changes: 27 additions & 50 deletions gtdynamics.i
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,24 +16,19 @@ const gtsam::KeyFormatter GTDKeyFormatter;

/********************** factors **********************/
#include <gtdynamics/factors/JointMeasurementFactor.h>
template <JOINT>
class JointMeasurementFactor : gtsam::NonlinearFactor {
JointMeasurementFactor(gtsam::Key wTp_key, gtsam::Key wTc_key,
const gtsam::noiseModel::Base *cost_model,
const gtdynamics::Joint *joint,
const JOINT::JointCoordinate &joint_coordinate,
size_t k);
double joint_coordinate);
JointMeasurementFactor(const gtsam::noiseModel::Base::shared_ptr &model,
const gtdynamics::Joint *joint,
const JOINT::JointCoordinate &joint_coordinate,
size_t k);
double joint_coordinate, size_t k);

void print(const string &s = "", const gtsam::KeyFormatter &keyFormatter =
gtdynamics::GTDKeyFormatter);
};

typedef gtdynamics::JointMeasurementFactor<gtdynamics::RevoluteJoint> RevoluteJointMeasurementFactor;

#include <gtdynamics/factors/PoseFactor.h>
class PoseFactor : gtsam::NonlinearFactor {
PoseFactor(gtsam::Key wTp_key, gtsam::Key wTc_key, gtsam::Key q_key,
Expand Down Expand Up @@ -104,10 +99,11 @@ class TwistFactor : gtsam::NonlinearFactor {

#include <gtdynamics/factors/TwistAccelFactor.h>
class TwistAccelFactor : gtsam::NonlinearFactor {
TwistAccelFactor(gtsam::Key twist_key_c, gtsam::Key twistAccel_key_p, gtsam::Key twistAccel_key_c,
gtsam::Key q_key, gtsam::Key qVel_key, gtsam::Key qAccel_key,
const gtsam::noiseModel::Base* cost_model,
const gtdynamics::JointTyped* joint);
TwistAccelFactor(gtsam::Key twist_key_c, gtsam::Key twistAccel_key_p,
gtsam::Key twistAccel_key_c, gtsam::Key q_key,
gtsam::Key qVel_key, gtsam::Key qAccel_key,
const gtsam::noiseModel::Base *cost_model,
const gtdynamics::Joint *joint);

void print(const string &s="",
const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
Expand All @@ -116,7 +112,7 @@ class TwistAccelFactor : gtsam::NonlinearFactor {
#include <gtdynamics/factors/TorqueFactor.h>
class TorqueFactor : gtsam::NonlinearFactor {
TorqueFactor(const gtsam::noiseModel::Base *cost_model,
const gtdynamics::JointTyped *joint, size_t k=0);
const gtdynamics::Joint *joint, size_t k=0);

void print(const string &s="",
const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
Expand Down Expand Up @@ -145,15 +141,15 @@ class WrenchFactor : gtsam::NonlinearFactor {
#include <gtdynamics/factors/WrenchEquivalenceFactor.h>
class WrenchEquivalenceFactor : gtsam::NonlinearFactor{
WrenchEquivalenceFactor(const gtsam::noiseModel::Base *cost_model,
gtdynamics::JointTyped *joint, size_t k = 0);
gtdynamics::Joint *joint, size_t k = 0);
void print(const string &s="",
const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
};

#include <gtdynamics/factors/WrenchPlanarFactor.h>
class WrenchPlanarFactor : gtsam::NonlinearFactor {
WrenchPlanarFactor(const gtsam::noiseModel::Base *cost_model,
Vector planar_axis, gtdynamics::JointTyped *joint, size_t k=0);
Vector planar_axis, gtdynamics::Joint *joint, size_t k=0);
void print(const string &s="",
const gtsam::KeyFormatter &keyFormatter=gtdynamics::GTDKeyFormatter);
};
Expand Down Expand Up @@ -231,11 +227,9 @@ class Link {

/********************** joint **********************/
#include <gtdynamics/universal_robot/Joint.h>
#include <gtdynamics/universal_robot/JointTyped.h>
#include <gtdynamics/universal_robot/ScrewJointBase.h>
#include <gtdynamics/universal_robot/RevoluteJoint.h>
#include <gtdynamics/universal_robot/PrismaticJoint.h>
#include <gtdynamics/universal_robot/ScrewJoint.h>
#include <gtdynamics/universal_robot/HelicalJoint.h>
class JointParams {
JointParams();
double velocity_limit;
Expand All @@ -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,
Expand All @@ -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,
Expand All @@ -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,
Expand Down Expand Up @@ -720,49 +709,37 @@ gtdynamics::DynamicsSymbol PhaseKey(int k);
gtdynamics::DynamicsSymbol TimeKey(int t);

///////////////////// Key Methods /////////////////////
template<T = {double}>
void InsertJointAngle(gtsam::Values@ values, int j, int t, T value);
void InsertJointAngle(gtsam::Values@ values, int j, int t, double value);

template<T = {double}>
void InsertJointAngle(gtsam::Values @values, int j, T value);
void InsertJointAngle(gtsam::Values @values, int j, double value);

gtsam::Vector JointAngle(const gtsam::VectorValues &values, int j, int t);

template<T = {double}>
T JointAngle(const gtsam::Values &values, int j, int t);
double JointAngle(const gtsam::Values &values, int j, int t);

template<T = {double}>
void InsertJointVel(gtsam::Values @values, int j, int t, T value);
void InsertJointVel(gtsam::Values @values, int j, int t, double value);

template<T = {double}>
void InsertJointVel(gtsam::Values @values, int j, T value);
void InsertJointVel(gtsam::Values @values, int j, double value);

gtsam::Vector JointVel(const gtsam::VectorValues &values, int j, int t);

template<T = {double}>
T JointVel(const gtsam::Values &values, int j, int t);
double JointVel(const gtsam::Values &values, int j, int t);

template<T = {double}>
void InsertJointAccel(gtsam::Values @values, int j, int t, T value);
void InsertJointAccel(gtsam::Values @values, int j, int t, double value);

template<T = {double}>
void InsertJointAccel(gtsam::Values @values, int j, T value);
void InsertJointAccel(gtsam::Values @values, int j, double value);

gtsam::Vector JointAccel(const gtsam::VectorValues &values, int j, int t);

template<T = {double}>
T JointAccel(const gtsam::Values &values, int j, int t);
double JointAccel(const gtsam::Values &values, int j, int t);

template<T = {double}>
void InsertTorque(gtsam::Values @values, int j, int t, T value);
void InsertTorque(gtsam::Values @values, int j, int t, double value);

template<T = {double}>
void InsertTorque(gtsam::Values @values, int j, T value);
void InsertTorque(gtsam::Values @values, int j, double value);

gtsam::Vector Torque(const gtsam::VectorValues &values, int j, int t);

template<T = {double}>
T Torque(const gtsam::Values &values, int j, int t);
double Torque(const gtsam::Values &values, int j, int t);

void InsertPose(gtsam::Values @values, int i, int t, gtsam::Pose3 value);

Expand Down
10 changes: 5 additions & 5 deletions gtdynamics/cablerobot/src/cdpr_planar_sim.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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))
Expand Down Expand Up @@ -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
Expand Down
8 changes: 4 additions & 4 deletions gtdynamics/cablerobot/src/gerry00_planar_sim.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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():
Expand Down
Loading