diff --git a/include/spline_optimization.h b/include/spline_optimization.h new file mode 100644 index 0000000..2471258 --- /dev/null +++ b/include/spline_optimization.h @@ -0,0 +1,245 @@ +/* + * TODO: This license is not consistent with license used in the project. + * Delete the inconsistent license and above line and rerun pre-commit to + * insert a good license. Copyright © 2022 Dexai Robotics. All rights reserved. + */ + +// drake headers + +#pragma once +#include +#include + +#include "drake/common/autodiff.h" +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/math/autodiff_gradient.h" + +#include + +#include + + + +// dexai headers +// #include "constraint_solver.h" +#include "drac_types.h" +#include "constraint_solver.h" +#include "parameters.h" + +using PPType_ad = drake::trajectories::PiecewisePolynomial; +using WAYPTS_ad = std::vector>; +using params = parameters::Parameters; + +// +// template +// class JointLimitChecker { +// public: +// // JointLimitCost(); +// JointLimitChecker(const drake::VectorX& lower_limit, +// const drake::VectorX& upper_limit); +// void Update(T distance, size_t i); + +// inline bool AreLimitsSatisfied() { +// return *min_distance > 0; +// } +// inline T* GetCost() { +// return cost.get(); +// } +// inline T GetMinDistance() { +// return *min_distance; +// } + +// private: +// const drake::VectorX lower_limit_; +// const drake::VectorX upper_limit_; +// std::unique_ptr cost = std::make_unique(static_cast(0)); +// // T* cost_ = static_cast(0); +// // This is ony for verification and user information purposes +// std::unique_ptr min_distance = +// std::make_unique(static_cast(std::numeric_limits::max())); +// // T* min_distance = static_cast(std::numeric_limits::max()); +// double joint_limit_threshold {0.05}; +// }; + +// This object is responsible for encoding constraints. +template +class ConstraintChecker { + /** + * @brief This class is responsible for encoding constraints. + * + * @param min_distance_ is the minimum margin within all constraints. + * @param cost_ is the cost function for this constraint. + + */ + private: + T cost_; + T min_distance_; + std::vector distances_ {}; + + const std::string name_; + double activation_threshold_; + double repel_threshold_; + + public: + ConstraintChecker(const std::string& name, double activation_threshold = 0.015, + double repel_threshold = 0.0); + + void Push(T distance) { + if (distance < min_distance_) { + min_distance_ = distance; + } + distances_.push_back(distance); + } + + inline void Reset() { + distances_.clear(); + cost_ = static_cast(0); + min_distance_ = std::numeric_limits::max(); + std::cout<<"Reset:: repel_threshold_ = "<< repel_threshold_ << std::endl; + }; + + inline void ResetHard() { + distances_.clear(); + cost_ = static_cast(0); + min_distance_ = std::numeric_limits::max(); + repel_threshold_ = 0.0; + }; + + inline T GetCost() { + return cost_; + } + + inline T GetMinimumDistance() { + return min_distance_; + } + + inline void SetInfinityCost() { + cost_ = std::numeric_limits::max(); + } + + void Evaluate(); + + // Public Parameters + double activation_threshold() const { + return activation_threshold_; + } + double repel_threshold() const { + return repel_threshold_; + } + inline void set_repel_threshold(double repel_threshold) { + repel_threshold_ = repel_threshold; + } +}; + +// To come soon! +// template +// T CalcSplineTime(drake::trajectories::PiecewisePolynomial& path, const +// Eigen::VectorXd& vlim, const Eigen::VectorXd& alim); + +/* SplineOptimizer +This class acts as an optimizer + */ +class SplineOptimizer { + public: + // Constructor + SplineOptimizer(); + + SplineOptimizer(std::vector>& waypts); + + SplineOptimizer(std::vector>& waypts, + std::shared_ptr cs, + const params& parameters); + + SplineOptimizer(const system_poly_t& syspoly, std::shared_ptr cs, const params& parameters, int n_waypts=10); + + /** + @brief Computes ... + The method is based on checking the extremum points of 3rd degree + piecewise polynomial. Therefore, it is exact. + */ + template + void CalcBoxLimitsCost(drake::trajectories::PiecewisePolynomial& spline, + Eigen::VectorXd& upper_limit, + Eigen::VectorXd& lower_limit); + + template + void CalcJointLimitsCost(drake::trajectories::PiecewisePolynomial& spline); + + // destructor + ~SplineOptimizer() { + std::cout << "SplineOptimizer Closed" << std::endl; + } + + template + void CalcCollisionCost(drake::trajectories::PiecewisePolynomial& spline, + int n_points = 100); + + + // drake::solvers::LinearConstraint AddProximityConstraint(system_conf_t sysconf); + + // void ProjectPoint(FeasibleSet); + + template + void CalcCollisionCost(std::vector>& waypts_vec); + + void AddAnchorPoint(); + + // Optimizes the spline. + void Optimize(int max_iterations = 30); + +// Uses Backtracking line search to select the gradient step size +// Look at, e.g., https://en.wikipedia.org/wiki/Backtracking_line_search +double BackTracking(drake::AutoDiffXd current_cost, double step, + double alpha = 0.5, double c = 0.5); + +// Implements one step of the optimization algorithm +void DescentOneStep(); + +public: + +template +T CalcSplineTime(drake::trajectories::PiecewisePolynomial& spline); + +template +T CalcApproximateSplineTime(drake::trajectories::PiecewisePolynomial& spline); + +template +T CalcApproximateSplineTime(drake::trajectories::PiecewisePolynomial& spline, const Eigen::VectorXd& vlim, + const Eigen::VectorXd& alim); + +private: +template +T CalcApproximateMultiRobotSplineTime(drake::trajectories::PiecewisePolynomial& spline); + +public: +// Helper function for getting a pointer to the current spline + inline PPType_ad* GetSpline() { + return &spline_; + } + + public: + robot_state_vec_t GetPlan(); + std::pair, std::vector> min_distance_trajectory_; + + // Hyper parameters + double collision_cutoff {0.02}; + double jointlimit_cutoff {0.02}; + bool convergence_ {false}; + + private: + // Initialized with class constructor + std::vector> waypts_; + std::shared_ptr cs_; // shared ptr + size_t dim_; // dimension of the spline space + params parameters_; + std::unique_ptr> collision_checker_ptr_, + jointlimit_checker_ptr_; // owned ptrs for autodiff spline + std::unique_ptr> collision_checker_temp_ptr_, + jointlimit_checker_temp_ptr_; // owned ptrs for double spline + const double collision_check_threshold {0.02}; // 5cm + double min_collision_distance {std::numeric_limits::max()}; + std::vector breaks_; + std::vector breaks_double_; + WAYPTS_ad waypts_ad_, waypts_ad_temp_; + PPType_ad spline_, spline_temp_; +}; diff --git a/src/spline_optimization b/src/spline_optimization new file mode 100644 index 0000000..e229093 --- /dev/null +++ b/src/spline_optimization @@ -0,0 +1,810 @@ +/* + * TODO: This license is not consistent with license used in the project. + * Delete the inconsistent license and above line and rerun pre-commit to + * insert a good license. Copyright © 2022 Dexai Robotics. All rights reserved. + */ + +#include /* atan2 */ + +// drake headers +#include + +#include "drake/common/autodiff.h" +#include "drake/common/trajectories/piecewise_polynomial.h" +#include "drake/math/autodiff_gradient.h" + +// dexai headers +#include "constraint_solver.h" +#include "drac_types.h" +#include "spline_optimization.h" + +template +ConstraintChecker::ConstraintChecker(const std::string& name, + double activation_threshold, + double repel_threshold) + : cost_ {static_cast(0)}, + min_distance_ {static_cast(std::numeric_limits::max())}, + name_ {name}, + activation_threshold_ {activation_threshold}, + repel_threshold_ {repel_threshold} { + log()->info( + "ConstraintChecker::Built an constraint checker for {} with activation " + "at {}", + name_, activation_threshold_); +} + + +// template +template <> +void ConstraintChecker::Evaluate() { + auto start = std::chrono::high_resolution_clock::now(); + double min_distance_value {min_distance_.value()}; + repel_threshold_ = std::min(min_distance_value, repel_threshold_); + log()->warn("ConstraintChecker::{} repelling threshold is {}", name_, + repel_threshold_); + for (size_t i = 0; i < distances_.size(); i++) { + cost_ += 1 / Eigen::pow(distances_[i] - repel_threshold_ + 0.001, 2); + } + log()->debug( + "ConstraintChecker::{} cost is {} and Minimum Distance is {} [mm]", name_, + cost_, min_distance_value * 1000); + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = + std::chrono::duration_cast(stop - start); + log()->debug("ConstraintChecker:: Evaluated in {} microseconds", + duration.count()); + distances_.clear(); +} + +template <> +void ConstraintChecker::Evaluate() { + for (size_t i = 0; i < distances_.size(); i++) { + if (distances_[i] > repel_threshold_) { + cost_ += 1 / pow(distances_[i] - repel_threshold_ + 0.001, 2); + } else { + cost_ = std::numeric_limits::max(); + break; + } + } + log()->debug("ConstraintChecker::{} cost is {} and Minimum Distance is {}", + name_, cost_, min_distance_); + distances_.clear(); +} + +SplineOptimizer::SplineOptimizer(std::vector>& waypts) { + waypts_ = waypts; + dim_ = static_cast(waypts_[0].size()); + for (size_t k {0}; k < waypts_.size(); k++) { + breaks_.push_back(static_cast(k)); + drake::MatrixX sample_ad(dim_, 1); + for (size_t i {}; i < dim_; i++) { + sample_ad(i, 0) = drake::AutoDiffXd(waypts_[k](i, 0), + dim_ * waypts_.size(), dim_ * k + i); + } + waypts_ad_.push_back(sample_ad); + } + spline_ = PPType_ad::CubicWithContinuousSecondDerivatives(breaks_, waypts_ad_, + Eigen::MatrixXd::Zero(dim_, 1), Eigen::MatrixXd::Zero(dim_, 1)); + momap::log()->info("Built an SplineOptimizer with Box Limits!"); +} + +SplineOptimizer::SplineOptimizer(std::vector>& waypts, + std::shared_ptr cs, + const params& parameters) + : waypts_ {waypts}, + cs_ {cs}, + dim_ {static_cast(waypts_[0].size())}, + parameters_ {parameters} { + for (size_t k {0}; k < waypts_.size(); k++) { + breaks_.push_back(static_cast(k)); + breaks_double_.push_back(static_cast(k)); + drake::MatrixX sample_ad(dim_, 1); + for (size_t i {}; i < dim_; i++) { + sample_ad(i, 0) = drake::AutoDiffXd(waypts_[k](i, 0), + dim_ * waypts_.size(), dim_ * k + i); + } + waypts_ad_.push_back(sample_ad); + } + spline_ = PPType_ad::CubicWithContinuousSecondDerivatives(breaks_, waypts_ad_, + Eigen::MatrixXd::Zero(dim_, 1), Eigen::MatrixXd::Zero(dim_, 1)); + collision_checker_ptr_ = + std::make_unique>("CollisionChecker", + collision_cutoff); + jointlimit_checker_ptr_ = + std::make_unique>( + "JointLimitsChecker", jointlimit_cutoff); + collision_checker_temp_ptr_ = std::make_unique>( + "Tempcollision_checker", collision_cutoff); + jointlimit_checker_temp_ptr_ = std::make_unique>( + "TempJointLimit", jointlimit_cutoff); + momap::log()->info( + "Built a spline optimizer from waypts with Dracula! \n dim= {} " + "#waypts={}", + dim_, waypts_.size()); +} + +SplineOptimizer::SplineOptimizer(const system_poly_t& syspoly, + std::shared_ptr cs, + const params& parameters, + int n_waypts) { + cs_ = cs; + parameters_ = parameters; + double t_start {syspoly.begin()->second.start_time()}; + double t_end {syspoly.begin()->second.end_time()}; + double time_step {(t_end - t_start) / (n_waypts-1) + 0.00001}; + auto sysconf_vec {dru::ToSysConfVec(syspoly, time_step)}; + for (auto sysconf : sysconf_vec) { + Eigen::VectorXd q_config {cs_->PackModelAccelerations(sysconf)}; + waypts_.push_back(q_config); + } + dim_ = static_cast(waypts_[0].size()); + // Now the usual construction + const auto v_start {Eigen::MatrixXd::Zero(dim_, 1)}; + const auto v_end {Eigen::MatrixXd::Zero(dim_, 1)}; + for (size_t k {0}; k < waypts_.size(); k++) { + breaks_.push_back(static_cast(k * time_step)); + breaks_double_.push_back(static_cast(k * time_step)); + drake::MatrixX sample_ad(dim_, 1); + for (size_t i {}; i < dim_; i++) { + sample_ad(i, 0) = drake::AutoDiffXd(waypts_[k](i, 0), + dim_ * waypts_.size(), dim_ * k + i); + } + waypts_ad_.push_back(sample_ad); + } + spline_ = PPType_ad::CubicWithContinuousSecondDerivatives(breaks_, waypts_ad_, + v_start, v_end); + collision_checker_ptr_ = + std::make_unique>( + "collision_checker", collision_cutoff); + jointlimit_checker_ptr_ = + std::make_unique>( + "JointLimitsChecker", jointlimit_cutoff); + collision_checker_temp_ptr_ = std::make_unique>( + "Tempcollision_checker", collision_cutoff); + jointlimit_checker_temp_ptr_ = std::make_unique>( + "TempJointLimitsChecker", jointlimit_cutoff); + momap::log()->info( + "Built a spline optimizer from a syspoly with Dracula! \n dim= {} " + "#waypts={}", + dim_, waypts_.size()); +} + +template <> +void SplineOptimizer::CalcBoxLimitsCost( + drake::trajectories::PiecewisePolynomial& spline, + Eigen::VectorXd& upper_limit, Eigen::VectorXd& lower_limit) { + jointlimit_checker_ptr_->Reset(); + for (size_t i {}; i < dim_; i++) { + for (size_t k = 0; k < breaks_.size() - 1; k++) { + // std::cout << "i = " << i << " and k = " << k << std::endl; + drake::Polynomial my_poly = spline.getPolynomialMatrix(k)(i, 0); + const auto cof = my_poly.GetCoefficients(); + // Solve the second root problem + auto delta_t = breaks_[k + 1] - breaks_[k]; + auto delta_squared = Eigen::pow(cof(2), 2) - 3 * cof(1) * cof(3); + std::vector candidates; // All points with + // derivative=0 + candidates.push_back(0); + if (k == breaks_.size() - 2) { + candidates.push_back(delta_t); + } // consider last point + if (std::abs(cof(3).value()) < 1e-6 && std::abs(cof(2).value()) > 1e-6) { + candidates.push_back(-cof(1) / 2 / cof(2)); + // std::cout << "degree 1" << std::endl; + } else if (delta_squared.value() > 0) { + // the roots are real + const auto root_1 = (-cof(2) + Eigen::sqrt(delta_squared)) / 3 / cof(3); + const auto root_2 = (-cof(2) - Eigen::sqrt(delta_squared)) / 3 / cof(3); + // std::cout << "roots are = " << root_1 << " and " << root_2 << + // std::endl; + if (root_1 > 0 && root_1 < delta_t) { + candidates.push_back(root_1); + } + if (root_2 > 0 && root_2 < delta_t) { + candidates.push_back(root_2); + } + // std::cout << "roots are = " << root_1 << " and " << root_2 << + // std::endl; + } + for (const auto& t : candidates) { + auto val = cof(0) + cof(1) * t + cof(2) * Eigen::pow(t, 2) + + cof(3) * Eigen::pow(t, 3); + // std::cout << "t = " << t << " val = " << val << std::endl; + if (upper_limit(i) - val + < jointlimit_checker_ptr_->activation_threshold()) { + jointlimit_checker_ptr_->Push(upper_limit(i) - val); + ; + } + if (val - lower_limit(i) + < jointlimit_checker_ptr_->activation_threshold()) { + jointlimit_checker_ptr_->Push(val - lower_limit(i)); + ; + } + } + } + } + jointlimit_checker_ptr_->Evaluate(); + jointlimit_checker_temp_ptr_->set_repel_threshold( + jointlimit_checker_ptr_->repel_threshold()); +} + +template <> +void SplineOptimizer::CalcBoxLimitsCost( + drake::trajectories::PiecewisePolynomial& spline, + Eigen::VectorXd& upper_limit, Eigen::VectorXd& lower_limit) { + jointlimit_checker_temp_ptr_->Reset(); + for (size_t i {}; i < dim_; i++) { + for (size_t k = 0; k < breaks_double_.size() - 1; k++) { + // std::cout << "i = " << i << " and k = " << k << std::endl; + drake::Polynomial my_poly = spline.getPolynomialMatrix(k)(i, 0); + const auto cof = my_poly.GetCoefficients(); + // Solve the second root problem + auto delta_t = breaks_double_[k + 1] - breaks_double_[k]; + auto delta_squared = std::pow(cof(2), 2) - 3 * cof(1) * cof(3); + std::vector candidates; // All points with + // derivative=0 + candidates.push_back(0); + if (k == breaks_.size() - 2) { + candidates.push_back(delta_t); + } // consider last point + if (std::abs(cof(3)) < 1e-6 && std::abs(cof(2)) > 1e-6) { + candidates.push_back(-cof(1) / 2 / cof(2)); + } else if (delta_squared > 0) { + // the roots are real + const auto root_1 = (-cof(2) + std::sqrt(delta_squared)) / 3 / cof(3); + const auto root_2 = (-cof(2) - std::sqrt(delta_squared)) / 3 / cof(3); + if (root_1 > 0 && root_1 < delta_t) { + candidates.push_back(root_1); + } + if (root_2 > 0 && root_2 < delta_t) { + candidates.push_back(root_2); + } + } + for (const auto& t : candidates) { + auto val = cof(0) + cof(1) * t + cof(2) * std::pow(t, 2) + + cof(3) * std::pow(t, 3); + if (upper_limit(i) - val + < jointlimit_checker_temp_ptr_->activation_threshold()) { + jointlimit_checker_temp_ptr_->Push(upper_limit(i) - val); + ; + } + if (val - lower_limit(i) + < jointlimit_checker_temp_ptr_->activation_threshold()) { + jointlimit_checker_temp_ptr_->Push(val - lower_limit(i)); + ; + } + } + } + } + jointlimit_checker_temp_ptr_->Evaluate(); +} + +template <> +void SplineOptimizer::CalcJointLimitsCost( + drake::trajectories::PiecewisePolynomial& spline) { + const auto& cobot_name {parameters_.GetCobotName()}; + const auto& aa_name {parameters_.GetAncillaryArmName()}; + const auto cobot_joint_limits { + parameters_.planning_limits_map.at(cobot_name)->joint_limits}; + const auto aa_joint_limits { + parameters_.planning_limits_map.at(aa_name)->joint_limits}; + system_conf_t sys_conf_upper_limit, sys_conf_lower_limit; + sys_conf_upper_limit[cobot_name] = cobot_joint_limits.upper_limits; + sys_conf_lower_limit[cobot_name] = cobot_joint_limits.lower_limits; + sys_conf_upper_limit[aa_name] = aa_joint_limits.upper_limits; + sys_conf_lower_limit[aa_name] = aa_joint_limits.lower_limits; + Eigen::VectorXd upper_limit = + cs_->PackModelAccelerations(sys_conf_upper_limit); + Eigen::VectorXd lower_limit = + cs_->PackModelAccelerations(sys_conf_lower_limit); + CalcBoxLimitsCost(spline, upper_limit, lower_limit); +} + +template <> +void SplineOptimizer::CalcJointLimitsCost( + drake::trajectories::PiecewisePolynomial& spline) { + const auto& cobot_name {parameters_.GetCobotName()}; + const auto& aa_name {parameters_.GetAncillaryArmName()}; + const auto cobot_joint_limits { + parameters_.planning_limits_map.at(cobot_name)->joint_limits}; + const auto aa_joint_limits { + parameters_.planning_limits_map.at(aa_name)->joint_limits}; + system_conf_t sys_conf_upper_limit, sys_conf_lower_limit; + sys_conf_upper_limit[cobot_name] = cobot_joint_limits.upper_limits; + sys_conf_lower_limit[cobot_name] = cobot_joint_limits.lower_limits; + sys_conf_upper_limit[aa_name] = aa_joint_limits.upper_limits; + sys_conf_lower_limit[aa_name] = aa_joint_limits.lower_limits; + Eigen::VectorXd upper_limit = + cs_->PackModelAccelerations(sys_conf_upper_limit); + Eigen::VectorXd lower_limit = + cs_->PackModelAccelerations(sys_conf_lower_limit); + CalcBoxLimitsCost(spline, upper_limit, lower_limit); +} + +template <> +void SplineOptimizer::CalcCollisionCost( + std::vector>& waypts_vec) { + auto start = std::chrono::high_resolution_clock::now(); + std::vector distance_vector; + collision_checker_temp_ptr_->Reset(); + system_conf_t sys_conf {cs_->SysConfZero()}; + for (size_t k {1}; k < waypts_vec.size() -1; k++) { + auto waypt {waypts_vec[k]}; + cs_->UnpackModelAccelerations(waypt, sys_conf); + auto dist_vec {cs_->GetCollisionGradients( + sys_conf, collision_checker_ptr_->activation_threshold())}; + for (auto& dist : dist_vec) { + if (collision_checker_temp_ptr_->repel_threshold() + > dist.value() + 0.001) { + momap::log()->trace("k={}, d = {}, repel_threshold = {}", k, + dist.value(), + collision_checker_temp_ptr_->repel_threshold()); + collision_checker_temp_ptr_->SetInfinityCost(); + return; + } else { + momap::log()->trace("k={}, d = {}", k, dist.value()); + collision_checker_temp_ptr_->Push(dist.value()); + } + } + } + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = + std::chrono::duration_cast(stop - start); + log()->debug("Collisions at {} points evaluated in {} microseconds", + waypts_vec.size(), duration.count()); + collision_checker_temp_ptr_->Evaluate(); +} + +template <> +void SplineOptimizer::CalcCollisionCost( + std::vector>& waypts_vec) { + auto start = std::chrono::high_resolution_clock::now(); + std::vector distance_vector; + collision_checker_ptr_->Reset(); + system_conf_t sys_conf {cs_->SysConfZero()}; + // For recording the distance-to-collision trajectory + min_distance_trajectory_.first.clear(); + min_distance_trajectory_.second.clear(); + for (size_t k {0}; k < waypts_vec.size(); k++) { + auto waypt {waypts_vec[k]}; + auto q_value = drake::math::ExtractValue(waypt); + cs_->UnpackModelAccelerations(q_value, sys_conf); + auto knot_dist_vec {cs_->GetCollisionGradients( + sys_conf, collision_checker_ptr_->activation_threshold())}; + double min_distance_knot {collision_checker_ptr_->activation_threshold()}; + for (auto& dist : knot_dist_vec) { + Eigen::VectorXd gradient { + Eigen::VectorXd::Zero(dim_ * waypts_vec.size())}; + if (dist.value()>0){ + gradient.segment(k * dim_, dim_) = dist.derivatives(); + } + else { + gradient.segment(k * dim_, dim_) = dist.derivatives(); + log()->warn("made the gradient reverse {}", dist.value()); + } + drake::AutoDiffXd distance_autodiff_spline = + Eigen::MakeAutoDiffScalar(dist.value(), gradient); + collision_checker_ptr_->Push(distance_autodiff_spline); + min_distance_knot = std::min(min_distance_knot, dist.value()); + } + min_distance_trajectory_.first.push_back(k); + min_distance_trajectory_.second.push_back(min_distance_knot + * 1000); // m to mm + } + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = + std::chrono::duration_cast(stop - start); + log()->debug("Collisions at {} points evaluated in {} microseconds", + waypts_vec.size(), duration.count()); + collision_checker_ptr_->Evaluate(); + collision_checker_temp_ptr_->set_repel_threshold( + collision_checker_ptr_->repel_threshold()); +} + +template <> +void SplineOptimizer::CalcCollisionCost( + drake::trajectories::PiecewisePolynomial& spline, int n_points) { + auto start = std::chrono::high_resolution_clock::now(); + EASY_BLOCK("CalcCollisionCost Autodiff") + // Divide the spline into segments + double t_start {spline.start_time()}; + double t_end {spline.end_time()}; + const auto check_times {Eigen::VectorXd::LinSpaced(n_points, t_start, t_end)}; + collision_checker_temp_ptr_->Reset(); + system_conf_t sys_conf {cs_->SysConfZero()}; + for (auto t : check_times) { + drake::MatrixX q_value = spline.value(t); + cs_->UnpackModelAccelerations(q_value, sys_conf); + auto dist_vec {cs_->GetCollisionGradients( + sys_conf, collision_checker_temp_ptr_->activation_threshold())}; + for (auto& dist : dist_vec) { + if (collision_checker_temp_ptr_->repel_threshold() + > dist.value() + 0.001) { + momap::log()->error("t={}, d = {}, repel_threshold = {}", t, + dist.value(), + collision_checker_temp_ptr_->repel_threshold()); + collision_checker_temp_ptr_->SetInfinityCost(); + return; + } else { + momap::log()->trace("t={}, d = {}", t, dist.value()); + collision_checker_temp_ptr_->Push(dist.value()); + } + } + } + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = + std::chrono::duration_cast(stop - start); + log()->debug( + "CalcCollisionCost Collision gradients at {} points evaluated in {} " + "microseconds", + n_points, duration.count()); + EASY_END_BLOCK; + collision_checker_temp_ptr_->Evaluate(); +} + +template <> +void SplineOptimizer::CalcCollisionCost( + drake::trajectories::PiecewisePolynomial& spline, + int n_points) { + auto start = std::chrono::high_resolution_clock::now(); + // For recording the distance-to-collision trajectory + min_distance_trajectory_.first.clear(); + min_distance_trajectory_.second.clear(); + // Divide the spline into segments + double t_start = (spline.start_time()).value(); + double t_end {(spline.end_time()).value()}; + const auto check_times {Eigen::VectorXd::LinSpaced(n_points, t_start, t_end)}; + collision_checker_ptr_->Reset(); + system_conf_t sys_conf {cs_->SysConfZero()}; + for (auto t : check_times) { + drake::MatrixX q_autodiff = spline.value(t); + auto q_value = drake::math::ExtractValue(q_autodiff); + cs_->UnpackModelAccelerations(q_value, sys_conf); + auto dist_vec {cs_->GetCollisionGradients( + sys_conf, collision_checker_ptr_->activation_threshold())}; + double min_distance_knot {collision_checker_ptr_->activation_threshold()}; + for (auto& dist : dist_vec) { + if (min_distance_knot > dist.value()) { + min_distance_knot = dist.value(); + } + momap::log()->trace("t={}, d = {}", t, dist.value()); + auto dist_derivative_q = dist.derivatives(); + Eigen::VectorXd gradient { + Eigen::VectorXd::Zero(q_autodiff(0).derivatives().size())}; + for (size_t j {0}; j < dim_; j++) { + // chain rule to get the gradient of the distance function with respect + // to the spline waypts + gradient += dist_derivative_q(j, 0) * q_autodiff(j, 0).derivatives(); + } + drake::AutoDiffXd distance_autodiff_spline = + Eigen::MakeAutoDiffScalar(dist.value(), gradient); + collision_checker_ptr_->Push(distance_autodiff_spline); + } + min_distance_trajectory_.first.push_back(t); + min_distance_trajectory_.second.push_back(min_distance_knot + * 1000); // m to mm + } + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = + std::chrono::duration_cast(stop - start); + log()->debug( + "CalcCollisionCost Collision gradients at {} points evaluated in {} " + "microseconds", + n_points, duration.count()); + collision_checker_ptr_->Evaluate(); + collision_checker_temp_ptr_->set_repel_threshold( + collision_checker_ptr_->repel_threshold()); +} + +drake::AutoDiffXd softmin(drake::AutoDiffXd x, drake::AutoDiffXd y, double a = 1) { + if (x < 0.5 * y) { + return x; + } else if (x > 2 * y) { + return y; + } else { + return (x * Eigen::exp(-a * x) + y * Eigen::exp(-a * y)) + / (Eigen::exp(-a * x) + Eigen::exp(-a * y)); + } +} + +double softmin(double x, double y, double a = 1) { + if (x < 0.5 * y) { + return x; + } else if (x > 2 * y) { + return y; + } else { + return (x * std::exp(-a * x) + y * std::exp(-a * y)) + / (std::exp(-a * x) + std::exp(-a * y)); + } +} + +template <> +drake::AutoDiffXd SplineOptimizer::CalcApproximateSplineTime(drake::trajectories::PiecewisePolynomial& spline, const Eigen::VectorXd& vlim, + const Eigen::VectorXd& alim) { + drake::AutoDiffXd time {0}; + double t_start{spline.start_time().value()}; + double t_end{spline.end_time().value()}; + int n_knots {300}; + const auto knots {Eigen::VectorXd::LinSpaced(n_knots, t_start, t_end)}; + for (int k = 0; k < n_knots - 1; k++) { + auto s = knots(k); + auto s_next = knots(k + 1); + auto delta = s_next - s; + auto qs_dot {spline.EvalDerivative(s, 1)}; + auto qs_ddot {spline.EvalDerivative(s, 2)}; + drake::AutoDiffXd sdot {std::numeric_limits::max()}; + for (size_t i = 0; i < dim_; i++) { + // Constraint 1: velocity limits + sdot = softmin(sdot, vlim(i) / Eigen::abs(qs_dot(i))); + // // Constraint 2: acceleration limits at current time + sdot = softmin(sdot, Eigen::sqrt(alim(i) / Eigen::abs(qs_ddot(i)))); + } + auto delta_time = delta / sdot; + time += delta_time; + } + return time; +} + +template <> +double SplineOptimizer::CalcApproximateSplineTime(drake::trajectories::PiecewisePolynomial& spline, const Eigen::VectorXd& vlim, + const Eigen::VectorXd& alim) { + EASY_FUNCTION(profiler::colors::Cyan); + double time {0}; + auto t_start{spline.start_time()}; + auto t_end{spline.end_time()}; + int n_knots {300}; + const auto knots {Eigen::VectorXd::LinSpaced(n_knots, t_start, t_end)}; + for (int k = 0; k < n_knots - 1; k++) { + auto s = knots(k); + auto s_next = knots(k + 1); + auto delta = s_next - s; + auto qs_dot {spline.EvalDerivative(s, 1)}; + auto qs_ddot {spline.EvalDerivative(s, 2)}; + auto sdot {std::numeric_limits::max()}; + for (size_t i = 0; i < dim_; i++) { + // Constraint 1: velocity limits + sdot = softmin(sdot, vlim(i) / std::abs(qs_dot(i))); + // // Constraint 2: acceleration limits at current time + sdot = softmin(sdot, std::sqrt((alim(i) / std::abs(qs_ddot(i))))); + } + auto delta_time = delta / sdot; + time += delta_time; + } + return time; +} + +template +T SplineOptimizer::CalcApproximateMultiRobotSplineTime(drake::trajectories::PiecewisePolynomial& spline){ + const auto& cobot_name {parameters_.GetCobotName()}; + const auto& aa_name {parameters_.GetAncillaryArmName()}; + system_conf_t sys_conf_vel_limit, sys_conf_acc_limit; + sys_conf_vel_limit [cobot_name] = + parameters_.planning_limits_map.at(cobot_name)->velocity_limits; + sys_conf_vel_limit [aa_name] = + parameters_.planning_limits_map.at(aa_name)->velocity_limits; + sys_conf_acc_limit [cobot_name] = + parameters_.planning_limits_map.at(cobot_name)->acceleration_limits; + sys_conf_acc_limit [aa_name] = + parameters_.planning_limits_map.at(aa_name)->acceleration_limits; + const Eigen::VectorXd vel_limit {cs_->PackModelAccelerations(sys_conf_vel_limit)}; + const Eigen::VectorXd acc_limit {cs_->PackModelAccelerations(sys_conf_acc_limit)}; + return CalcApproximateSplineTime(spline, vel_limit, acc_limit); +} + +template<> +double SplineOptimizer::CalcSplineTime(drake::trajectories::PiecewisePolynomial& spline){ + return CalcApproximateMultiRobotSplineTime(spline); +} + +template<> +drake::AutoDiffXd SplineOptimizer::CalcSplineTime(drake::trajectories::PiecewisePolynomial& spline){ + return CalcApproximateMultiRobotSplineTime(spline); +} + +double SplineOptimizer::BackTracking(drake::AutoDiffXd current_cost, + double initial_step, double alpha, double c) { + // Normalize the gradient + EASY_FUNCTION(profiler::colors::Magenta); + auto gradient {current_cost.derivatives().transpose()}; + // don't move initial and final points + gradient.segment(0, dim_) = Eigen::VectorXd::Zero(dim_, 1); + gradient.segment(dim_ * (waypts_ad_.size() - 1), dim_) = + Eigen::VectorXd::Zero(dim_, 1); + // Normalize the gradient + if (gradient.lpNorm() > 1e-6) { + gradient /= gradient.lpNorm(); + } else { + log()->warn("gradient is too small"); + return 0; + } + log()->debug("gradient = {}",gradient); + // Backtracking + auto step {initial_step}; + for (int iter {0}; iter < 20; iter++) { + std::vector> waypts; + for (size_t k {0}; k < waypts_ad_.size(); k++) { + waypts.push_back(drake::math::ExtractValue(waypts_ad_[k])); + for (size_t i {0}; i < dim_; i++) { + if (k == 0 || k == waypts_ad_.size() - 1) { + continue; + } else { + waypts[k](i, 0) = + waypts_ad_[k](i, 0).value() - step * gradient(dim_ * k + i); + } + } + log()->debug("step = {} , k= {}, \n waypts = {}, \n waypt_ad = {}, \n diff = {}", step, k, waypts[k].transpose(), + waypts_ad_[k].transpose(), waypts[k].transpose() - waypts_ad_[k].transpose()); + + } + auto spline_temp {PPType::CubicWithContinuousSecondDerivatives( + breaks_double_, waypts, Eigen::MatrixXd::Zero(dim_, 1), + Eigen::MatrixXd::Zero(dim_, 1))}; + CalcCollisionCost(spline_temp, 100); + // CalcCollisionCost(waypts); + CalcJointLimitsCost(spline_temp); + double collision_cost_temp {collision_checker_temp_ptr_->GetCost()}; + double jointlimit_cost_temp {jointlimit_checker_temp_ptr_->GetCost()}; + double time_cost {CalcSplineTime(spline_temp)}; + double cost_temp {collision_cost_temp + jointlimit_cost_temp + 1000*time_cost}; + std::cout << "Temporary collision cost: " << collision_cost_temp + << " minimum collision margin = " + << collision_checker_temp_ptr_->GetMinimumDistance() << std::endl; + std::cout << "Temporary jointlimit cost: " << jointlimit_cost_temp + << " minimum jointlimit margin = " + << jointlimit_checker_temp_ptr_->GetMinimumDistance() + << std::endl; + std::cout << "Temporary time cost: " << 1000*time_cost << std::endl; + std::cout << "Temporary cost: " << cost_temp << std::endl; + if (current_cost.value() - cost_temp + >= c * step * std::pow(gradient.norm(), 2)) { + return step; + } else { + log()->debug("Backtracking decrease in cost: {} <=> threshold {}", + current_cost.value() - cost_temp, + c * step * std::pow(gradient.norm(), 2)); + log()->debug("Backtracking: step {} iter = {}", step, iter); + step = alpha * step; + if (iter==10) { + log()->warn("Searching the reverse direction"); + step = -initial_step; + } + } + } + return 0; +} + +void SplineOptimizer::DescentOneStep() { + CalcJointLimitsCost(spline_); + CalcCollisionCost(spline_, 100); + // CalcCollisionCost(waypts_ad_); + drake::AutoDiffXd jointlimit_cost {jointlimit_checker_ptr_->GetCost()}; + drake::AutoDiffXd collision_cost {collision_checker_ptr_->GetCost()}; + drake::AutoDiffXd time_cost {CalcSplineTime(spline_)}; + drake::AutoDiffXd current_cost {collision_cost + jointlimit_cost + 1000*time_cost}; + std::cout << "\t *** collision cost: " << collision_cost + << " minimum collision margin = " + << collision_checker_ptr_->GetMinimumDistance() << std::endl; + std::cout << "\t *** jointlimit cost: " << jointlimit_cost + << " minimum jointlimit margin = " + << jointlimit_checker_ptr_->GetMinimumDistance() << std::endl; + std::cout << "\t *** cost: " << current_cost << std::endl; + if (current_cost.value() == 0) { + log()->critical("Optimization succesfull!"); + convergence_ = true; + return; + } + double step {BackTracking(current_cost, 0.7, 0.6, 0.8)}; + if (step == 0) { + log()->critical( + "Backtracking failed. This seems to be a local optimum. Stopping " + "optimization."); + convergence_ = true; + return; + } + // Normalize the gradient + auto gradient {current_cost.derivatives()}; + // don't move initial and final points + gradient.segment(0, dim_) = Eigen::VectorXd::Zero(dim_, 1); + gradient.segment(dim_ * (waypts_ad_.size() - 1), dim_) = + Eigen::VectorXd::Zero(dim_, 1); + // Normalize the gradient + if (gradient.lpNorm() > 1e-6) { + gradient /= gradient.lpNorm(); + } + for (size_t k {0}; k < waypts_ad_.size(); k++) { + for (size_t i {0}; i < dim_; i++) { + if (k == 0 || k == waypts_ad_.size() - 1) { + continue; + } else { + waypts_ad_[k](i, 0) -= step * gradient(dim_ * k + i); + } + } + } + spline_ = PPType_ad::CubicWithContinuousSecondDerivatives( + breaks_, waypts_ad_, Eigen::MatrixXd::Zero(dim_, 1), + Eigen::MatrixXd::Zero(dim_, 1)); + collision_checker_ptr_->ResetHard(); +} + +void SplineOptimizer::Optimize(int max_iterations) { + for (int i {0}; i < max_iterations; i++) { + if (convergence_) { + break; + } + DescentOneStep(); + } +} + +robot_state_vec_t SplineOptimizer::GetPlan() { + robot_state_vec_t plan; + for (auto waypt_ad : waypts_ad_) { + auto waypt {drake::math::ExtractValue(waypt_ad)}; + system_conf_t sys_conf {cs_->SysConfZero()}; + cs_->UnpackModelAccelerations(waypt, sys_conf); + plan.push_back(cs_->ToState(sys_conf)); + } + return plan; +}; + + +// From the another file +std::vector SystemModel::CalcCollisionGradients( + const system_conf_t& sys_conf, const double distance_threshold) { + auto signed_distance_pairs { + RetrieveSignedDistancePairs(sys_conf, distance_threshold)}; + + // Query port used to find out results from scene graph + const auto& query_port = robots_plant_->get_geometry_query_input_port(); + + if (!query_port.HasValue(*plant_context_)) { + throw std::invalid_argument( + "Cannot get a valid geometry::QueryObject. " + "Either the plant geometry_query_input_port() is not properly " + "connected to the SceneGraph's output port, or the *plant_context_ is " + "incorrect. Please refer to AddMultibodyPlantSceneGraph on connecting " + "MultibodyPlant to SceneGraph."); + } + + const auto& query_object = + query_port.Eval>(*plant_context_); + const auto& inspector = query_object.inspector(); + + auto start = std::chrono::high_resolution_clock::now(); + std::vector collision_distances; + for (const auto& signed_distance_pair : signed_distance_pairs) { + const auto& frameA = robots_plant_ + ->GetBodyFromFrameId(inspector.GetFrameId( + signed_distance_pair.id_A)) + ->body_frame(); + const auto& frameB = robots_plant_ + ->GetBodyFromFrameId(inspector.GetFrameId( + signed_distance_pair.id_B)) + ->body_frame(); + auto p_ACa = signed_distance_pair.p_ACa; + auto nhat_BA_W = signed_distance_pair.nhat_BA_W; + auto distance = signed_distance_pair.distance; + drake::AutoDiffXd distance_autodiff; + + auto q = robots_plant_->GetPositions(*plant_context_); + log()->debug("q={}", q.transpose()); + drake::VectorX q_ad(q.size()); + for (int i = 0; i < q.size(); i++) { + drake::AutoDiffXd temp(q(0), q.size(), i); + q_ad(i) = temp; + } + drake::multibody::internal::CalcDistanceDerivatives( + *robots_plant_, *plant_context_, frameA, frameB, p_ACa, distance, + nhat_BA_W, q_ad, &distance_autodiff); + log()->debug("Distance between {} and {} = {} \nDistance Gradient={}", + robots_plant_->GetBodyFromFrameId(inspector.GetFrameId(signed_distance_pair.id_A))->name(), + robots_plant_->GetBodyFromFrameId(inspector.GetFrameId(signed_distance_pair.id_B))->name(), + signed_distance_pair.distance, distance_autodiff.derivatives().transpose()); + collision_distances.push_back(distance_autodiff); + } + auto stop = std::chrono::high_resolution_clock::now(); + auto duration = + std::chrono::duration_cast(stop - start); + log()->debug("checked {} pairs in {} microseconds", + signed_distance_pairs.size(), duration.count()); + return collision_distances; +} \ No newline at end of file diff --git a/test_aa_optimization_read_only.cc b/test_aa_optimization_read_only.cc new file mode 100644 index 0000000..dacf1c2 --- /dev/null +++ b/test_aa_optimization_read_only.cc @@ -0,0 +1,293 @@ +/* + * Copyright © 2022 Dexai Robotics. All rights reserved. + */ + +#include "drake/solvers/ipopt_solver.h" +#include "mock_dracula.h" +#include "path_optimization.h" + +TEST(Optimization, TestAA) { + parameters::Parameters params; + auto pdrac { + dut::make_dracula(params, "/src/config/franka_aa.yaml", false, 0, 2)}; + params.urdf = + "/src/catkin_src/salad_bar_description/urdf/" + ".go_hotel_pan_third_6in_000_disher_2oz-drake.urdf"; + params.UpdateUrdf(params.urdf); + pdrac->ResetParams(params, true); + pdrac->StartMeshcatVisualizer(); + robot_state_vec_t traj; + std::string mpac_filepath = "test_data/example_traj.mpac"; + dru::load_msg_pack(traj, mpac_filepath); + momap::log()->info("traj size: {}", traj.size()); + const auto syspoly {dru::ToSysPoly(traj, params)}; + momap::log()->info("syspoly size: {}", syspoly.size()); + double T {dru::Duration(syspoly)}; + momap::log()->info("syspoly time : {}", T); + // Setup Robot Diagram + auto pop = std::make_unique(params); + const auto& plant = pop->GetPlant(); + auto& mutable_plant_context = pop->MutuablePlantContext(); + // Setup joint limits + const auto& cobot_name {params.GetCobotName()}; + const auto& aa_name {params.GetAncillaryArmName()}; + system_conf_t sys_conf_joint_lim_lower, sys_conf_joint_lim_upper, + sys_conf_vel_limit, sys_conf_acc_limit; + sys_conf_joint_lim_lower[cobot_name] = + params.planning_limits_map.at(cobot_name)->joint_limits.lower_limits; + sys_conf_joint_lim_upper[cobot_name] = + params.planning_limits_map.at(cobot_name)->joint_limits.upper_limits; + sys_conf_joint_lim_lower[aa_name] = + params.planning_limits_map.at(aa_name)->joint_limits.lower_limits; + sys_conf_joint_lim_upper[aa_name] = + params.planning_limits_map.at(aa_name)->joint_limits.upper_limits; + sys_conf_vel_limit[cobot_name] = + params.planning_limits_map.at(cobot_name)->velocity_limits; + sys_conf_vel_limit[aa_name] = + params.planning_limits_map.at(aa_name)->velocity_limits; + sys_conf_acc_limit[cobot_name] = + params.planning_limits_map.at(cobot_name)->acceleration_limits; + sys_conf_acc_limit[aa_name] = + params.planning_limits_map.at(aa_name)->acceleration_limits; + const Eigen::VectorXd joint_limit_lower { + pdrac->GetCS()->PackModelAccelerations(sys_conf_joint_lim_lower)}; + const Eigen::VectorXd joint_limit_upper { + pdrac->GetCS()->PackModelAccelerations(sys_conf_joint_lim_upper)}; + const Eigen::VectorXd joint_vel_limit { + pdrac->GetCS()->PackModelAccelerations(sys_conf_vel_limit)}; + const Eigen::VectorXd joint_acc_limit { + pdrac->GetCS()->PackModelAccelerations(sys_conf_acc_limit)}; + const auto joint_limits {anzu::planning::JointLimits( + joint_limit_lower, joint_limit_upper, -joint_vel_limit, joint_vel_limit, + -joint_acc_limit, joint_acc_limit)}; + // Geometry port for collisions + const auto& query_port {plant.get_geometry_query_input_port()}; + // ************************************************************************** + // ************************* Set up the robot ****************************** + // ************************************************************************** + drake::MatrixX q_franka = Eigen::VectorXd::Zero(7); + drake::MatrixX q_aa = Eigen::VectorXd::Zero(2); + auto franka_model {pop->model_map_[cobot_name]}; + auto aa_model {pop->model_map_[aa_name]}; + auto prog = std::make_unique(); + // ************************************************************************** + // ******************** Set up the System Polynomial *********************** + // ************************************************************************** + // Step 1: Setup the AA polynomial + std::vector> aa_vars_vec; + std::vector breaks_vec; + int n_aa_vars = 5; + { + q_aa = (syspoly.at(params.GetAncillaryArmName())).value(0); + drake::MatrixX aa_start {q_aa}; + aa_vars_vec.push_back(aa_start); + auto t_start = drake::symbolic::Expression {0.0}; + breaks_vec.push_back(t_start); + } + for (int i = 1; i < n_aa_vars - 2; i++) { + auto q_var = prog->NewContinuousVariables(2, "q_aa_" + std::to_string(i)); + aa_vars_vec.push_back(q_var); + const double t {i * T / (n_aa_vars - 1)}; + breaks_vec.push_back(drake::symbolic::Expression(t)); + } + { + q_aa = (syspoly.at(params.GetAncillaryArmName())).value(T); + drake::MatrixX aa_end {q_aa}; + aa_vars_vec.push_back(aa_end); + auto t_dispense = + drake::symbolic::Expression {(n_aa_vars - 2) * T / (n_aa_vars - 1)}; + breaks_vec.push_back(t_dispense); + } + { + q_aa = (syspoly.at(params.GetAncillaryArmName())).value(T); + drake::MatrixX aa_end {q_aa}; + aa_vars_vec.push_back(aa_end); + auto t_end = drake::symbolic::Expression {T}; + breaks_vec.push_back(t_end); + } + // Step 3: Build the AA polynomial from the AA variables + auto aa_symbolic_poly = + drake::trajectories::PiecewisePolynomial:: + CubicWithContinuousSecondDerivatives(breaks_vec, aa_vars_vec, + Eigen::VectorXd::Zero(2), + Eigen::VectorXd::Zero(2)); + momap::log()->debug("aa_symbolic_poly: {}", + aa_symbolic_poly.value(0.0).transpose()); + momap::log()->debug("aa_symbolic_poly: {}", + aa_symbolic_poly.value(0.1).transpose()); + // Step 4: Setup the Franka polynomial + int n_franka_pts = 20; + std::vector> franka_vec; + std::vector> aa_double_vec; + std::vector franka_breaks_vec; + for (int i = 0; i < n_franka_pts; i++) { + const double t {i * T / (n_franka_pts - 1)}; + q_franka = (syspoly.at(params.GetCobotName())).value(t); + q_aa = (syspoly.at(params.GetAncillaryArmName())).value(t); + franka_vec.push_back(q_franka); + aa_double_vec.push_back(q_aa); + franka_breaks_vec.push_back(t); + } + auto franka_double_poly = drake::trajectories::PiecewisePolynomial< + double>::CubicWithContinuousSecondDerivatives(franka_breaks_vec, + franka_vec); + auto aa_double_poly = drake::trajectories::PiecewisePolynomial< + double>::CubicWithContinuousSecondDerivatives(franka_breaks_vec, + aa_double_vec); + momap::log()->info("franka_double_poly: {}", + franka_double_poly.value(0.7).transpose()); + momap::log()->info("aa_double_poly: {}", + aa_double_poly.value(0.7).transpose()); + // Step 5: Sample the franka_polynomial and the aa_polynomial to construct + // collision avoidance constraint + momap::log()->info("lower joints = {}", + joint_limits.position_lower().transpose()); + momap::log()->info("higher joints = {}", + joint_limits.position_upper().transpose()); + int n_sample = 50; + for (int i = 0; i < n_sample; i++) { + const double t {i * T / (n_sample - 1)}; + q_franka = franka_double_poly.value(t); + q_aa = aa_double_poly.value(t); + plant.SetPositions(&mutable_plant_context, franka_model, q_franka); + plant.SetPositions(&mutable_plant_context, aa_model, q_aa); + auto q_vars { + prog->NewContinuousVariables(9, 1, "q_collision " + std::to_string(i))}; + auto con {std::shared_ptr( + new drake::multibody::MinimumDistanceConstraint( + &plant, 0.01, &mutable_plant_context, {}, 0.15))}; + prog->AddBoundingBoxConstraint(joint_limits.position_lower(), + joint_limits.position_upper(), q_vars); + // Bond the q_vars to the aa_polynomial + momap::log()->info("q_aa: {}, {}", i, q_aa.transpose()); + for (size_t j = 0; j < 2; j++) { + prog->AddLinearEqualityConstraint( + aa_symbolic_poly.value(t)(j, 0) - q_vars(7 + j, 0), 0.0); + } + momap::log()->info("q_franka: {}, {}", i, q_franka.transpose()); + for (size_t j = 0; j < 7; j++) { + prog->AddLinearEqualityConstraint(q_vars(j, 0), q_franka(j, 0)); + } + auto constraint {prog->AddConstraint(con, q_vars)}; + // Collision check print here + auto q {plant.GetPositions(mutable_plant_context)}; + momap::log()->warn("Collision constraint satisfied at sample {}: {}", i, + con->CheckSatisfied(q, 0.0)); + const auto& query_object { + query_port.Eval>( + mutable_plant_context)}; + const auto& inspector {query_object.inspector()}; + auto distance_pairs { + query_object.ComputeSignedDistancePairwiseClosestPoints(0.01)}; + for (const auto& dist_pair : distance_pairs) { + const auto name_body_1 { + plant.GetBodyFromFrameId(inspector.GetFrameId(dist_pair.id_A)) + ->name()}; + const auto name_body_2 { + plant.GetBodyFromFrameId(inspector.GetFrameId(dist_pair.id_B)) + ->name()}; + momap::log()->info("{} / {} :{}", name_body_1, name_body_2, + dist_pair.distance); + } + } + // Step 6: Add time cost! + drake::symbolic::Expression time_cost {0.0}; + momap::log()->info("time_cost: {}", time_cost.to_string()); + // Just doing this for AA for now + int n_time_samples {10}; + for (int i = 0; i < n_time_samples; i++) { + const drake::symbolic::Expression s {i * T / (n_time_samples - 1)}; + // const double delta_s {T / (n_time_samples - 1)}; + auto q_der_s {aa_symbolic_poly.EvalDerivative(s, 1)}; + auto q_der2_s {aa_symbolic_poly.EvalDerivative(s, 2)}; + // drake::symbolic::Expression sdot {std::numeric_limits::max()}; + // for (size_t j = 0; j < 2; j++){ + // sdot = drake::symbolic::min(sdot, joint_limits.velocity_upper()(j) / + // q_der_s(j,0)); sdot = drake::symbolic::min(sdot, + // drake::symbolic::sqrt(joint_limits.acceleration_upper()(j) / + // q_der2_s(j,0))); + // } + // TODO(@sadraddini) is not a polynomial. ParseCost does not support + // non-polynomial expression. + drake::symbolic::Expression sdot; + for (size_t j = 0; j < 2; j++) { + sdot += 10 * q_der2_s(j, 0) * q_der2_s(j, 0); + sdot += 1000 * q_der_s(j, 0) * q_der_s(j, 0); + } + time_cost += sdot; + } + prog->AddCost(time_cost); + // Step 7: Set initial guess + for (int i = 1; i < n_aa_vars - 2; i++) { + const double t {i * T / (n_aa_vars - 1)}; + q_aa = aa_double_poly.value(t); + for (size_t j = 0; j < 2; j++) { + auto vars = aa_vars_vec[i](j, 0).GetVariables(); + momap::log()->info("vars: {}, {}, {}", i, j, q_aa(j, 0)); + auto var = *vars.begin(); + prog->SetInitialGuess(var, q_aa(j, 0)); + } + } + // Step 8: Solve! + drake::solvers::SnoptSolver solver; + // drake::solvers::IpoptSolver solver; + const std::string print_file = "snopt.out"; + drake::solvers::SolverOptions solver_options; + solver_options.SetOption(drake::solvers::SnoptSolver::id(), "Print file", + print_file); + // solver_options.SetOption(drake::solvers::CommonSolverOption::kPrintFileName, + // "snopting.out"); + const auto result = solver.Solve(*prog, {}, solver_options); + // drake::solvers::MathematicalProgramResult result = solver.Solve(*prog, {}, + // {}); + momap::log()->info("Solver used: {}", result.get_solver_id().name()); + momap::log()->critical("Solver success: {}", result.is_success()); + momap::log()->critical("Solver success: {}", result.get_solution_result()); + + if (result.is_success()) { + momap::log()->info("Optimal cost is {}", result.get_optimal_cost()); + // Now: let's go back and spline the optimized decisions! + // Step 1: Construct the optimal AA spline + std::vector> aa_optimized_q_vec; + std::vector aa_optimized_t_vec; + for (int i = 0; i < n_aa_vars; i++) { + drake::MatrixX q_optimized = + drake::symbolic::Evaluate(result.GetSolution(aa_vars_vec[i])); + aa_optimized_q_vec.push_back(q_optimized); + aa_optimized_t_vec.push_back(i * T / (n_aa_vars - 1)); + momap::log()->info("q_optimized {}: {}", i, q_optimized.transpose()); + } + auto aa_optimized_q_poly = drake::trajectories::PiecewisePolynomial< + double>::CubicWithContinuousSecondDerivatives(aa_optimized_t_vec, + aa_optimized_q_vec, + Eigen::VectorXd::Zero(2), + Eigen::VectorXd::Zero(2)); + // Step 2: Sample from the optimized AA spline and franka_spline to + // construct a plan + int n_sample_plan {20}; + robot_state_vec_t plan; + for (int i = 0; i < n_sample_plan; i++) { + system_conf_t sys_conf; + const double t {i * T / (n_sample_plan - 1)}; + sys_conf[cobot_name] = franka_double_poly.value(t); + sys_conf[aa_name] = aa_optimized_q_poly.value(t); + plan.push_back(pdrac->GetCS()->ToState(sys_conf)); + } + const auto optimized_syspoly { + pdrac->GetTS()->TimeOptimalSpline(plan, 1, 1, true, true, true)}; + double T_optimized {dru::Duration(optimized_syspoly)}; + momap::log()->info("syspoly time : old: {} vs new: {}", T, T_optimized); + // display results in meshcat: + pdrac->GetMeshcat()->DisplaySysPoly(syspoly, "original trajectory"); + pdrac->GetMeshcat()->DisplaySysPoly(optimized_syspoly, + "optimized trajectory"); + } else { + momap::log()->warn("Solver failed"); + auto infeasible_vec {result.GetInfeasibleConstraints(*prog)}; + momap::log()->info("Infeasible constraints size: {}", + infeasible_vec.size()); + for (const auto& infeasible : infeasible_vec) { + momap::log()->info("Infeasible constraint: {}", infeasible.to_string()); + } + } +}