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

Jump to bottom

Fixed horizon and other bugs in examples #100

Merged
merged 9 commits into from
Apr 2, 2021
Merged

Fixed horizon and other bugs in examples #100

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
c1e14a8
Fixed horizon bug in hermite parametrization
scharalambous3 Mar 29, 2021
7def47d
Fixed issue due to updated constructor
scharalambous3 Mar 29, 2021
26d0e8d
Fixed another issue due to updated constructor
scharalambous3 Mar 29, 2021
c39830c
Fixed build errors for these examples
scharalambous3 Mar 29, 2021
3f57462
Improved documentation
scharalambous3 Mar 29, 2021
30d8475
Resolved review comments
scharalambous3 Mar 30, 2021
fbbf32f
use linear algebra for clean cubic spline computation
varunagrawal Mar 30, 2021
7a5765c
replace typedef with using
varunagrawal Mar 30, 2021
5fdc093
Changed Eigen comma-initializer syntax
scharalambous3 Mar 31, 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
40 changes: 20 additions & 20 deletions examples/example_cart_pole_trajectory_optimization/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -56,41 +56,41 @@ int main(int argc, char** argv) {
std::cout << "Goal State: " << X_T.transpose() << std::endl << std::endl;

// Create trajectory factor graph.
auto graph_builder = DynamicsGraph();
auto graph_builder = DynamicsGraph(gravity);
auto graph = graph_builder.trajectoryFG(
cp, t_steps, dt, DynamicsGraph::CollocationScheme::Trapezoidal, gravity);
cp, t_steps, dt, DynamicsGraph::CollocationScheme::Trapezoidal);

// Set the pendulum joint to be unactuated.
for (int t = 0; t <= t_steps; t++)
graph.addPrior(TorqueKey(j1_id, t), 0.0, Constrained::All(1));
graph.addPrior(internal::TorqueKey(j1_id, t), 0.0, Constrained::All(1));

// Add initial conditions to trajectory factor graph.
graph.addPrior(JointAngleKey(j0_id, 0), X_i[0], dynamics_model);
graph.addPrior(JointVelKey(j0_id, 0), X_i[1], dynamics_model);
graph.addPrior(JointAngleKey(j1_id, 0), X_i[3], dynamics_model);
graph.addPrior(JointVelKey(j1_id, 0), X_i[4], dynamics_model);
graph.addPrior(internal::JointAngleKey(j0_id, 0), X_i[0], dynamics_model);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Copy/pasta
I think there must be a function already to do this, and if not, there should be

graph.addPrior(internal::JointVelKey(j0_id, 0), X_i[1], dynamics_model);
graph.addPrior(internal::JointAngleKey(j1_id, 0), X_i[3], dynamics_model);
graph.addPrior(internal::JointVelKey(j1_id, 0), X_i[4], dynamics_model);

// Add terminal conditions to the factor graph.
graph.addPrior(JointVelKey(j0_id, t_steps), X_T[1], objectives_model);
graph.addPrior(JointAccelKey(j0_id, t_steps), X_T[2], objectives_model);
graph.addPrior(JointVelKey(j1_id, t_steps), X_T[4], objectives_model);
graph.addPrior(JointAccelKey(j1_id, t_steps), X_T[5], objectives_model);
graph.addPrior(internal::JointVelKey(j0_id, t_steps), X_T[1], objectives_model);
graph.addPrior(internal::JointAccelKey(j0_id, t_steps), X_T[2], objectives_model);
graph.addPrior(internal::JointVelKey(j1_id, t_steps), X_T[4], objectives_model);
graph.addPrior(internal::JointAccelKey(j1_id, t_steps), X_T[5], objectives_model);

// Insert position objective (x, theta) factor at every timestep or only at
// the terminal state. Adding the position objective at every timestep will
// force the system to converge to the desired state quicker at the cost of
// more impulsive control actions.
bool apply_pos_objective_all_dt = false;
graph.addPrior(JointAngleKey(j0_id, t_steps), X_T[0], pos_objectives_model);
graph.addPrior(JointAngleKey(j1_id, t_steps), X_T[3], pos_objectives_model);
graph.addPrior(internal::JointAngleKey(j0_id, t_steps), X_T[0], pos_objectives_model);
graph.addPrior(internal::JointAngleKey(j1_id, t_steps), X_T[3], pos_objectives_model);
if (apply_pos_objective_all_dt) {
for (int t = 0; t < t_steps; t++) {
graph.addPrior(JointAngleKey(j0_id, t), X_T[0], pos_objectives_model);
graph.addPrior(JointAngleKey(j1_id, t), X_T[3], pos_objectives_model);
graph.addPrior(internal::JointAngleKey(j0_id, t), X_T[0], pos_objectives_model);
graph.addPrior(internal::JointAngleKey(j1_id, t), X_T[3], pos_objectives_model);
}
}
for (int t = 0; t <= t_steps; t++)
graph.emplace_shared<MinTorqueFactor>(TorqueKey(j0_id, t), control_model);
graph.emplace_shared<MinTorqueFactor>(internal::TorqueKey(j0_id, t), control_model);

// Initialize solution.
auto init_vals = ZeroValuesTrajectory(cp, t_steps, 0, 0.0);
Expand All @@ -107,10 +107,10 @@ int main(int argc, char** argv) {
double t_elapsed = 0;
for (int t = 0; t <= t_steps; t++, t_elapsed += dt) {
std::vector<gtsam::Key> keys = {
JointAngleKey(j0_id, t), JointVelKey(j0_id, t),
JointAccelKey(j0_id, t), TorqueKey(j0_id, t),
JointAngleKey(j1_id, t), JointVelKey(j1_id, t),
JointAccelKey(j1_id, t), TorqueKey(j1_id, t)};
internal::JointAngleKey(j0_id, t), internal::JointVelKey(j0_id, t),
internal::JointAccelKey(j0_id, t), internal::TorqueKey(j0_id, t),
internal::JointAngleKey(j1_id, t), internal::JointVelKey(j1_id, t),
internal::JointAccelKey(j1_id, t), internal::TorqueKey(j1_id, t)};
scharalambous3 marked this conversation as resolved.
Show resolved Hide resolved
std::vector<std::string> vals = {std::to_string(t_elapsed)};
for (auto&& k : keys) vals.push_back(std::to_string(results.atDouble(k)));
traj_file << boost::algorithm::join(vals, ",") << "\n";
Expand Down
8 changes: 4 additions & 4 deletions examples/example_forward_dynamics/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,11 +12,11 @@ int main(int argc, char** argv) {
auto simple_rr = CreateRobotFromFile("../simple_rr.sdf", "simple_rr_sdf");
simple_rr.print();

auto graph_builder = DynamicsGraph();
gtsam::Vector3 gravity = (gtsam::Vector(3) << 0, 0, -9.8).finished();
auto graph_builder = DynamicsGraph(gravity);
auto kdfg = graph_builder.dynamicsFactorGraph(simple_rr,
0, // timestep
gravity);
0 // timestep
);

// Specify the forward dynamics priors and add them to the factor graph.
gtsam::Vector theta = (gtsam::Vector(2) << 0, 0).finished();
Expand All @@ -35,4 +35,4 @@ int main(int argc, char** argv) {
graph_builder.printValues(results);

return 0;
}
}
23 changes: 11 additions & 12 deletions examples/example_inverted_pendulum_trajectory_optimization/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,26 +52,25 @@ int main(int argc, char** argv) {
double theta_T = M_PI, dtheta_T = 0, ddtheta_T = 0;

// Create trajectory factor graph.
auto graph_builder = DynamicsGraph();
auto graph_builder = DynamicsGraph(gravity, planar_axis);
auto graph = graph_builder.trajectoryFG(
ip, t_steps, dt, DynamicsGraph::CollocationScheme::Trapezoidal, gravity,
planar_axis);
ip, t_steps, dt, DynamicsGraph::CollocationScheme::Trapezoidal);

// Add initial conditions to trajectory factor graph.
graph.addPrior(JointAngleKey(j1_id, 0), theta_i, dynamics_model);
graph.addPrior(JointVelKey(j1_id, 0), dtheta_i, dynamics_model);
graph.addPrior(internal::JointAngleKey(j1_id, 0), theta_i, dynamics_model);
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The same copy/pasta!!!!!!

graph.addPrior(internal::JointVelKey(j1_id, 0), dtheta_i, dynamics_model);

// Add state and min torque objectives to trajectory factor graph.
graph.addPrior(JointVelKey(j1_id, t_steps), dtheta_T, objectives_model);
graph.addPrior(JointAccelKey(j1_id, t_steps), dtheta_T, objectives_model);
graph.addPrior(internal::JointVelKey(j1_id, t_steps), dtheta_T, objectives_model);
graph.addPrior(internal::JointAccelKey(j1_id, t_steps), dtheta_T, objectives_model);
bool apply_theta_objective_all_dt = false;
graph.addPrior(JointAngleKey(j1_id, t_steps), theta_T, objectives_model);
graph.addPrior(internal::JointAngleKey(j1_id, t_steps), theta_T, objectives_model);
if (apply_theta_objective_all_dt) {
for (int t = 0; t < t_steps; t++)
graph.addPrior(JointAngleKey(j1_id, t), theta_T, objectives_model);
graph.addPrior(internal::JointAngleKey(j1_id, t), theta_T, objectives_model);
}
for (int t = 0; t <= t_steps; t++)
graph.emplace_shared<MinTorqueFactor>(TorqueKey(j1_id, t), control_model);
graph.emplace_shared<MinTorqueFactor>(internal::TorqueKey(j1_id, t), control_model);

// Initialize solution.
auto init_vals = ZeroValuesTrajectory(ip, t_steps, 0, 0.0);
Expand All @@ -88,8 +87,8 @@ int main(int argc, char** argv) {
double t_elapsed = 0;
for (int t = 0; t <= t_steps; t++, t_elapsed += dt) {
std::vector<gtsam::Key> keys = {
JointAngleKey(j1_id, t), JointVelKey(j1_id, t), JointAccelKey(j1_id, t),
TorqueKey(j1_id, t)};
internal::JointAngleKey(j1_id, t), internal::JointVelKey(j1_id, t), internal::JointAccelKey(j1_id, t),
internal::TorqueKey(j1_id, t)};
std::vector<std::string> vals = {std::to_string(t_elapsed)};
for (auto&& k : keys) vals.push_back(std::to_string(results.atDouble(k)));
traj_file << boost::algorithm::join(vals, ",") << "\n";
Expand Down
13 changes: 8 additions & 5 deletions examples/example_quadruped_mp/main.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,13 +78,13 @@ CoeffVector compute_spline_coefficients(const Pose3 &wTb_i, const Pose3 &wTb_f,
// respectively.
// Vectors a_0 to a_3 constitute the matrix product A of the basis, B, and control,
scharalambous3 marked this conversation as resolved.
Show resolved Hide resolved
// C, matrices leading to p(u)=U(u)*A where A=B*C.
Vector3 a_0 = x_0, a_1 = x_0_p;
Vector3 a_0 = x_0, a_1 = x_0_p/horizon;
scharalambous3 marked this conversation as resolved.
Show resolved Hide resolved
// Since u=t/horizon, rearrange to integrate the horizon in the matrix product
// of B and C.
Vector3 a_2 = -std::pow(horizon, -2) *
(3 * (x_0 - x_1) + horizon * (2 * x_0_p + x_1_p));
(3 * (x_0 - x_1) + (2 * x_0_p + x_1_p));
Vector3 a_3 =
std::pow(horizon, -3) * (2 * (x_0 - x_1) + horizon * (x_0_p + x_1_p));
std::pow(horizon, -3) * (2 * (x_0 - x_1) + (x_0_p + x_1_p));
scharalambous3 marked this conversation as resolved.
Show resolved Hide resolved

std::vector<gtsam::Vector> coeffs;
coeffs.push_back(a_0);
Expand All @@ -96,8 +96,11 @@ CoeffVector compute_spline_coefficients(const Pose3 &wTb_i, const Pose3 &wTb_f,
}

/**
* Compute the robot base pose as defined by the hermite spline.
* TODO(frank): document better
* Compute the robot base pose as defined by the calculated trajectory.
* The calculated trajectory is a piecewise polynomial consisting of
* cubic hermite splines; the base pose will move along this trajectory.
* The cubic hermite splines have C1 continuity i.e. continuous in position
* and tangent vector.
scharalambous3 marked this conversation as resolved.
Show resolved Hide resolved
*/
Pose3 compute_hermite_pose(const CoeffVector &coeffs, const Vector3 &x_0_p,
const double t, const Pose3 &wTb_i) {
Expand Down