Skip to content

Commit

Permalink
Merge pull request #100 from borglab/fix/quadruped
Browse files Browse the repository at this point in the history
  • Loading branch information
@dellaert
dellaert authored Apr 2, 2021
2 parents ddebd0e + 5fdc093 commit 1d601da
Show file tree
Hide file tree
Showing 4 changed files with 82 additions and 78 deletions.
40 changes: 20 additions & 20 deletions examples/example_cart_pole_trajectory_optimization/main.cpp
View file
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);
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)};
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
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::Values known_values;
Expand All @@ -39,4 +39,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
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);
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
89 changes: 47 additions & 42 deletions examples/example_quadruped_mp/main.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,9 +38,9 @@ using gtsam::Pose3;
using gtsam::Rot3;
using gtsam::Vector3;

typedef std::vector<gtsam::Vector> CoeffVector;
typedef std::map<int, std::map<std::string, Pose3>> TargetFootholds;
typedef std::map<std::string, Pose3> TargetPoses;
using CoeffMatrix = gtsam::Matrix43;
using TargetFootholds = std::map<int, std::map<std::string, Pose3>>;
using TargetPoses = std::map<std::string, Pose3>;

using namespace gtdynamics;

Expand All @@ -55,60 +55,67 @@ using namespace gtdynamics;
* @param x_1_p tangent at the ending knot
* @param horizon timestep between the knot at the start and the knot at the end
*
* @return 3*4 matrix of coefficients
*
* Since uE[0,1] in the parametrization of the hermite spline equation, for
* tE[0,horizon], u is set equal to t/horizon
* @return 4*3 matrix of coefficients.
*
* Since u ∈ [0,1] in the parametrization of the hermite spline equation, for
* t ∈ [0,horizon], u is set equal to `t/horizon`.
*
* Refer to this lecture for more info on the hermite parameterization
* for cubic polynomials:
* http://www.cs.cmu.edu/afs/cs/academic/class/15462-s10/www/lec-slides/lec06.pdf
*/
CoeffVector compute_spline_coefficients(const Pose3 &wTb_i, const Pose3 &wTb_f,
CoeffMatrix compute_spline_coefficients(const Pose3 &wTb_i, const Pose3 &wTb_f,
const Vector3 &x_0_p,
const Vector3 &x_1_p,
const double horizon) {

// Extract position at the starting and ending knot.
Vector3 x_0 = wTb_i.translation();
Vector3 x_1 = wTb_f.translation();

// Hermite parameterization: p(u)=U(u)*B*C where vector U(u) includes the
// Hermite parameterization: p(u)=U(u)*B*C where vector U(u) includes the
// polynomial terms, and B and C are the basis matrix and control matrices
// respectively.
// Vectors a_0 to a_3 constitute the matrix product A of the basis, B, and control,
// C, matrices leading to p(u)=U(u)*A where A=B*C.
Vector3 a_0 = x_0, a_1 = x_0_p;
// 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));
Vector3 a_3 =
std::pow(horizon, -3) * (2 * (x_0 - x_1) + horizon * (x_0_p + x_1_p));

std::vector<gtsam::Vector> coeffs;
coeffs.push_back(a_0);
coeffs.push_back(a_1);
coeffs.push_back(a_2);
coeffs.push_back(a_3);

return coeffs;
gtsam::Matrix43 C;
C.row(0) = x_0;
C.row(1) = x_1;
C.row(2) = x_0_p;
C.row(3) = x_1_p;

gtsam::Matrix4 B;
B << 2, -2, 1, 1, -3, 3, -2, -1, 0, 0, 1, 0, 1, 0, 0, 0;
gtsam::Matrix43 A = B * C;

// Scale U by the horizon `t = u/horizon` so that we can directly use t.
A.row(0) /= (horizon * horizon * horizon);
A.row(1) /= (horizon * horizon);
A.row(2) /= horizon;

return A;
}

/**
* 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.
*
* @param coeffs 4*3 matrix of cubic polynomial coefficients
* @param x_0_p tangent at the starting knot
* @param t time at which pose will be evaluated
* @param wTb_i initial pose corresponding to the knot at the start
*
* @return rotation and position
*/
Pose3 compute_hermite_pose(const CoeffVector &coeffs, const Vector3 &x_0_p,
Pose3 compute_hermite_pose(const CoeffMatrix &coeffs, const Vector3 &x_0_p,
const double t, const Pose3 &wTb_i) {
// The position computed from the spline equation as a function of time,
// p(t)=U(t)*A where U(t)=[1,t,t^2,t^3].
Point3 p(coeffs[0] + coeffs[1] * t + coeffs[2] * std::pow(t, 2) +
coeffs[3] * std::pow(t, 3));
// p(t)=U(t)*A where U(t)=[t^3,t^2,t,1].
gtsam::Matrix14 t_vec(std::pow(t, 3), std::pow(t, 2), t, 1);
Point3 p(t_vec * coeffs);

// Differentiate position with respect to t for velocity.
Point3 dpdt_v3 =
Point3(coeffs[1] + 2 * coeffs[2] * t + 3 * coeffs[3] * std::pow(t, 2));
gtsam::Matrix14 du(3 * t * t, 2 * t, 1, 0);
Point3 dpdt_v3 = Point3(du * coeffs);

// Unit vector for velocity.
dpdt_v3 = dpdt_v3 / dpdt_v3.norm();
Point3 x_0_p_point(x_0_p);
Expand All @@ -123,11 +130,10 @@ Pose3 compute_hermite_pose(const CoeffVector &coeffs, const Vector3 &x_0_p,
}

/** Compute the target footholds for each support phase. */
TargetFootholds
compute_target_footholds(const CoeffVector &coeffs, const Vector3 &x_0_p,
const Pose3 &wTb_i, const double horizon,
const double t_support,
const std::map<std::string, Pose3> &bTfs) {
TargetFootholds compute_target_footholds(
const CoeffMatrix &coeffs, const Vector3 &x_0_p, const Pose3 &wTb_i,
const double horizon, const double t_support,
const std::map<std::string, Pose3> &bTfs) {
TargetFootholds target_footholds;

double t_swing = t_support / 4.0; // Time for each swing foot trajectory.
Expand All @@ -153,7 +159,7 @@ TargetPoses compute_target_poses(const TargetFootholds &targ_footholds,
const double horizon, const double t_support,
const double t,
const std::vector<std::string> &swing_sequence,
const CoeffVector &coeffs,
const CoeffMatrix &coeffs,
const Vector3 &x_0_p, const Pose3 &wTb_i) {
TargetPoses t_poses;
// Compute the body pose.
Expand Down Expand Up @@ -221,7 +227,6 @@ struct CsvWriter {
~CsvWriter() { pose_file.close(); }

void writeheader() {
pose_file.open("../traj.csv");
pose_file << "bodyx,bodyy,bodyz";
for (auto &&leg : swing_sequence)
pose_file << "," << leg << "x"
Expand Down

0 comments on commit 1d601da

Please sign in to comment.