diff --git a/towr/include/towr/constraints/base_motion_constraint.h b/towr/include/towr/constraints/base_motion_constraint.h index eb9969c8e..30678de67 100644 --- a/towr/include/towr/constraints/base_motion_constraint.h +++ b/towr/include/towr/constraints/base_motion_constraint.h @@ -30,8 +30,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_CONSTRAINTS_BASE_MOTION_CONSTRAINT_H_ #define TOWR_CONSTRAINTS_BASE_MOTION_CONSTRAINT_H_ -#include #include +#include #include "time_discretization_constraint.h" @@ -46,26 +46,27 @@ namespace towr { * @ingroup Constraints */ class BaseMotionConstraint : public TimeDiscretizationConstraint { -public: + public: /** * @brief Links the base variables and sets hardcoded bounds on the state. * @param T The total time of the optimization horizon. * @param dt The discretization interval of the constraints. * @param spline_holder Holds pointers to the base variables. */ - BaseMotionConstraint (double T, double dt, const SplineHolder& spline_holder); - virtual ~BaseMotionConstraint () = default; + BaseMotionConstraint(double T, double dt, const SplineHolder& spline_holder); + virtual ~BaseMotionConstraint() = default; - void UpdateConstraintAtInstance (double t, int k, VectorXd& g) const override; - void UpdateBoundsAtInstance (double t, int k, VecBound&) const override; - void UpdateJacobianAtInstance(double t, int k, std::string, Jacobian&) const override; + void UpdateConstraintAtInstance(double t, int k, VectorXd& g) const override; + void UpdateBoundsAtInstance(double t, int k, VecBound&) const override; + void UpdateJacobianAtInstance(double t, int k, std::string, + Jacobian&) const override; -private: + private: NodeSpline::Ptr base_linear_; NodeSpline::Ptr base_angular_; - VecBound node_bounds_; ///< same bounds for each discretized node - int GetRow (int node, int dim) const; + VecBound node_bounds_; ///< same bounds for each discretized node + int GetRow(int node, int dim) const; }; } /* namespace towr */ diff --git a/towr/include/towr/constraints/dynamic_constraint.h b/towr/include/towr/constraints/dynamic_constraint.h index b75aea27a..7fe19e379 100644 --- a/towr/include/towr/constraints/dynamic_constraint.h +++ b/towr/include/towr/constraints/dynamic_constraint.h @@ -30,9 +30,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_CONSTRAINTS_DYNAMIC_CONSTRAINT_H_ #define TOWR_CONSTRAINTS_DYNAMIC_CONSTRAINT_H_ +#include #include #include -#include #include @@ -61,7 +61,7 @@ namespace towr { * @ingroup Constraints */ class DynamicConstraint : public TimeDiscretizationConstraint { -public: + public: using Vector6d = Eigen::Matrix; /** @@ -71,18 +71,17 @@ class DynamicConstraint : public TimeDiscretizationConstraint { * @param dt the discretization intervall at which to enforce constraints. * @param spline_holder A pointer to the current optimization variables. */ - DynamicConstraint (const DynamicModel::Ptr& model, - double T, double dt, - const SplineHolder& spline_holder); - virtual ~DynamicConstraint () = default; + DynamicConstraint(const DynamicModel::Ptr& model, double T, double dt, + const SplineHolder& spline_holder); + virtual ~DynamicConstraint() = default; -private: - NodeSpline::Ptr base_linear_; ///< lin. base pos/vel/acc in world frame - EulerConverter base_angular_; ///< angular base state - std::vector ee_forces_; ///< endeffector forces in world frame. - std::vector ee_motion_; ///< endeffector position in world frame. + private: + NodeSpline::Ptr base_linear_; ///< lin. base pos/vel/acc in world frame + EulerConverter base_angular_; ///< angular base state + std::vector ee_forces_; ///< eef forces in world frame. + std::vector ee_motion_; ///< eff position in world frame. - mutable DynamicModel::Ptr model_; ///< the dynamic model (e.g. Centroidal) + mutable DynamicModel::Ptr model_; ///< the dynamic model (e.g. Centroidal) /** * @brief The row in the overall constraint for this evaluation time. @@ -100,7 +99,8 @@ class DynamicConstraint : public TimeDiscretizationConstraint { void UpdateConstraintAtInstance(double t, int k, VectorXd& g) const override; void UpdateBoundsAtInstance(double t, int k, VecBound& bounds) const override; - void UpdateJacobianAtInstance(double t, int k, std::string, Jacobian&) const override; + void UpdateJacobianAtInstance(double t, int k, std::string, + Jacobian&) const override; }; } /* namespace towr */ diff --git a/towr/include/towr/constraints/force_constraint.h b/towr/include/towr/constraints/force_constraint.h index faf683a18..8772fe8fe 100644 --- a/towr/include/towr/constraints/force_constraint.h +++ b/towr/include/towr/constraints/force_constraint.h @@ -32,8 +32,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include // for friction cone #include -#include // for friction cone namespace towr { @@ -53,9 +53,9 @@ namespace towr { * @ingroup Constraints */ class ForceConstraint : public ifopt::ConstraintSet { -public: + public: using Vector3d = Eigen::Vector3d; - using EE = uint; + using EE = uint; /** * @brief Constructs a force contraint. @@ -63,26 +63,25 @@ class ForceConstraint : public ifopt::ConstraintSet { * @param force_limit_in_normal_direction Maximum pushing force [N]. * @param endeffector_id Which endeffector force should be constrained. */ - ForceConstraint (const HeightMap::Ptr& terrain, - double force_limit_in_normal_direction, - EE endeffector_id); - virtual ~ForceConstraint () = default; + ForceConstraint(const HeightMap::Ptr& terrain, + double force_limit_in_normal_direction, EE endeffector_id); + virtual ~ForceConstraint() = default; void InitVariableDependedQuantities(const VariablesPtr& x) override; VectorXd GetValues() const override; VecBound GetBounds() const override; - void FillJacobianBlock (std::string var_set, Jacobian&) const override; + void FillJacobianBlock(std::string var_set, Jacobian&) const override; -private: - NodesVariablesPhaseBased::Ptr ee_force_; ///< the current xyz foot forces. - NodesVariablesPhaseBased::Ptr ee_motion_; ///< the current xyz foot positions. + private: + NodesVariablesPhaseBased::Ptr ee_force_; ///< the current xyz foot forces. + NodesVariablesPhaseBased::Ptr ee_motion_; ///< the current xyz foot pos/ns. - HeightMap::Ptr terrain_; ///< gradient information at every position (x,y). - double fn_max_; ///< force limit in normal direction. - double mu_; ///< friction coeff between robot feet and terrain. - int n_constraints_per_node_; ///< number of constraint for each node. - EE ee_; ///< The endeffector force to be constrained. + HeightMap::Ptr terrain_; ///< gradient information at every position (x,y). + double fn_max_; ///< force limit in normal direction. + double mu_; ///< friction coeff between robot feet and terrain. + int n_constraints_per_node_; ///< number of constraint for each node. + EE ee_; ///< The endeffector force to be constrained. /** * The are those Hermite-nodes that shape the polynomial during the diff --git a/towr/include/towr/constraints/linear_constraint.h b/towr/include/towr/constraints/linear_constraint.h index 0b980616e..18a95d769 100644 --- a/towr/include/towr/constraints/linear_constraint.h +++ b/towr/include/towr/constraints/linear_constraint.h @@ -40,7 +40,7 @@ namespace towr { * @ingroup Constraints */ class LinearEqualityConstraint : public ifopt::ConstraintSet { -public: + public: using MatrixXd = Eigen::MatrixXd; /** @@ -50,16 +50,15 @@ class LinearEqualityConstraint : public ifopt::ConstraintSet { * @param v The vector v defining the constanct offset. * @param variable_set The name of the variables x. */ - LinearEqualityConstraint (const MatrixXd& M, - const VectorXd& v, - const std::string& variable_set); - virtual ~LinearEqualityConstraint () = default; + LinearEqualityConstraint(const MatrixXd& M, const VectorXd& v, + const std::string& variable_set); + virtual ~LinearEqualityConstraint() = default; VectorXd GetValues() const final; VecBound GetBounds() const final; - void FillJacobianBlock (std::string var_set, Jacobian&) const final; + void FillJacobianBlock(std::string var_set, Jacobian&) const final; -private: + private: MatrixXd M_; VectorXd v_; std::string variable_name_; diff --git a/towr/include/towr/constraints/range_of_motion_constraint.h b/towr/include/towr/constraints/range_of_motion_constraint.h index 52fab8c53..7a723a2a5 100644 --- a/towr/include/towr/constraints/range_of_motion_constraint.h +++ b/towr/include/towr/constraints/range_of_motion_constraint.h @@ -30,9 +30,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_CONSTRAINTS_RANGE_OF_MOTION_CONSTRAINT_H_ #define TOWR_CONSTRAINTS_RANGE_OF_MOTION_CONSTRAINT_H_ +#include #include #include -#include #include @@ -53,8 +53,8 @@ namespace towr { * @ingroup Constraints */ class RangeOfMotionConstraint : public TimeDiscretizationConstraint { -public: - using EE = uint; + public: + using EE = uint; using Vector3d = Eigen::Vector3d; /** @@ -65,25 +65,25 @@ class RangeOfMotionConstraint : public TimeDiscretizationConstraint { * @param ee The endeffector for which to constrain the range. * @param spline_holder Pointer to the current variables. */ - RangeOfMotionConstraint(const KinematicModel::Ptr& robot_model, - double T, double dt, - const EE& ee, + RangeOfMotionConstraint(const KinematicModel::Ptr& robot_model, double T, + double dt, const EE& ee, const SplineHolder& spline_holder); virtual ~RangeOfMotionConstraint() = default; -private: - NodeSpline::Ptr base_linear_; ///< the linear position of the base. - EulerConverter base_angular_; ///< the orientation of the base. - NodeSpline::Ptr ee_motion_; ///< the linear position of the endeffectors. + private: + NodeSpline::Ptr base_linear_; ///< the linear position of the base. + EulerConverter base_angular_; ///< the orientation of the base. + NodeSpline::Ptr ee_motion_; ///< the linear position of the endeffectors. Eigen::Vector3d max_deviation_from_nominal_; Eigen::Vector3d nominal_ee_pos_B_; EE ee_; // see TimeDiscretizationConstraint for documentation - void UpdateConstraintAtInstance (double t, int k, VectorXd& g) const override; - void UpdateBoundsAtInstance (double t, int k, VecBound&) const override; - void UpdateJacobianAtInstance(double t, int k, std::string, Jacobian&) const override; + void UpdateConstraintAtInstance(double t, int k, VectorXd& g) const override; + void UpdateBoundsAtInstance(double t, int k, VecBound&) const override; + void UpdateJacobianAtInstance(double t, int k, std::string, + Jacobian&) const override; int GetRow(int node, int dimension) const; }; diff --git a/towr/include/towr/constraints/spline_acc_constraint.h b/towr/include/towr/constraints/spline_acc_constraint.h index 812176890..486db8c96 100644 --- a/towr/include/towr/constraints/spline_acc_constraint.h +++ b/towr/include/towr/constraints/spline_acc_constraint.h @@ -46,21 +46,21 @@ namespace towr { * @ingroup Constraints */ class SplineAccConstraint : public ifopt::ConstraintSet { -public: + public: SplineAccConstraint(const NodeSpline::Ptr& spline, std::string name); virtual ~SplineAccConstraint() = default; VectorXd GetValues() const override; VecBound GetBounds() const override; - void FillJacobianBlock (std::string var_set, Jacobian&) const override; + void FillJacobianBlock(std::string var_set, Jacobian&) const override; -private: - NodeSpline::Ptr spline_; ///< a spline comprised of polynomials - std::string node_variables_id_; /// polynomial parameterized node values + private: + NodeSpline::Ptr spline_; ///< a spline comprised of polynomials + std::string node_variables_id_; /// polynomial parameterized node values - int n_junctions_; ///< number of junctions between polynomials in spline. - int n_dim_; ///< dimensions that this polynomial represents (e.g. x,y). - std::vector T_; ///< Duration of each polynomial in spline. + int n_junctions_; ///< number of junctions between polynomials in spline. + int n_dim_; ///< dimensions that this polynomial represents (e.g. x,y). + std::vector T_; ///< Duration of each polynomial in spline. }; } /* namespace towr */ diff --git a/towr/include/towr/constraints/swing_constraint.h b/towr/include/towr/constraints/swing_constraint.h index 32006784a..48547605f 100644 --- a/towr/include/towr/constraints/swing_constraint.h +++ b/towr/include/towr/constraints/swing_constraint.h @@ -47,23 +47,23 @@ namespace towr { * @ingroup Constraints */ class SwingConstraint : public ifopt::ConstraintSet { -public: + public: using Vector2d = Eigen::Vector2d; /** * @brief Links the swing constraint with current foot variables. * @param ee_motion_id The name of the foot variables in the optimization. */ - SwingConstraint (std::string ee_motion_id); - virtual ~SwingConstraint () = default; + SwingConstraint(std::string ee_motion_id); + virtual ~SwingConstraint() = default; VectorXd GetValues() const override; VecBound GetBounds() const override; - void FillJacobianBlock (std::string var_set, Jacobian&) const override; + void FillJacobianBlock(std::string var_set, Jacobian&) const override; void InitVariableDependedQuantities(const VariablesPtr& x) override; -private: + private: NodesVariablesPhaseBased::Ptr ee_motion_; double t_swing_avg_ = 0.3; std::string ee_motion_id_; diff --git a/towr/include/towr/constraints/terrain_constraint.h b/towr/include/towr/constraints/terrain_constraint.h index 4656ff3b9..12c751ebf 100644 --- a/towr/include/towr/constraints/terrain_constraint.h +++ b/towr/include/towr/constraints/terrain_constraint.h @@ -32,8 +32,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include #include +#include namespace towr { @@ -49,7 +49,7 @@ namespace towr { * @ingroup Constraints */ class TerrainConstraint : public ifopt::ConstraintSet { -public: + public: using Vector3d = Eigen::Vector3d; /** @@ -57,21 +57,22 @@ class TerrainConstraint : public ifopt::ConstraintSet { * @param terrain The terrain height value and slope for each position x,y. * @param ee_motion_id The name of the endeffector variable set. */ - TerrainConstraint (const HeightMap::Ptr& terrain, std::string ee_motion_id); - virtual ~TerrainConstraint () = default; + TerrainConstraint(const HeightMap::Ptr& terrain, std::string ee_motion_id); + virtual ~TerrainConstraint() = default; void InitVariableDependedQuantities(const VariablesPtr& x) override; VectorXd GetValues() const override; VecBound GetBounds() const override; - void FillJacobianBlock (std::string var_set, Jacobian&) const override; + void FillJacobianBlock(std::string var_set, Jacobian&) const override; -private: - NodesVariablesPhaseBased::Ptr ee_motion_; ///< the position of the endeffector. - HeightMap::Ptr terrain_; ///< the height map of the current terrain. + private: + NodesVariablesPhaseBased::Ptr + ee_motion_; ///< the position of the endeffector. + HeightMap::Ptr terrain_; ///< the height map of the current terrain. - std::string ee_motion_id_; ///< the name of the endeffector variable set. - std::vector node_ids_; ///< the indices of the nodes constrained. + std::string ee_motion_id_; ///< the name of the endeffector variable set. + std::vector node_ids_; ///< the indices of the nodes constrained. }; } /* namespace towr */ diff --git a/towr/include/towr/constraints/time_discretization_constraint.h b/towr/include/towr/constraints/time_discretization_constraint.h index 94cce356e..53356c987 100644 --- a/towr/include/towr/constraints/time_discretization_constraint.h +++ b/towr/include/towr/constraints/time_discretization_constraint.h @@ -48,7 +48,7 @@ namespace towr { * @ingroup Constraints */ class TimeDiscretizationConstraint : public ifopt::ConstraintSet { -public: + public: using VecTimes = std::vector; using Bounds = ifopt::Bounds; @@ -58,25 +58,26 @@ class TimeDiscretizationConstraint : public ifopt::ConstraintSet { * @param dt The discretization interval at which each constraint is evaluated. * @param constraint_name The name of the constraint. */ - TimeDiscretizationConstraint (double T, double dt, std::string constraint_name); + TimeDiscretizationConstraint(double T, double dt, + std::string constraint_name); /** * @brief construct a constraint for ifopt. * @param dts Time instances at which to evaluate the constraints. * @param name The name of the constraint. */ - TimeDiscretizationConstraint (const VecTimes& dts, std::string name); - virtual ~TimeDiscretizationConstraint () = default; + TimeDiscretizationConstraint(const VecTimes& dts, std::string name); + virtual ~TimeDiscretizationConstraint() = default; Eigen::VectorXd GetValues() const override; VecBound GetBounds() const override; - void FillJacobianBlock (std::string var_set, Jacobian&) const override; + void FillJacobianBlock(std::string var_set, Jacobian&) const override; -protected: + protected: int GetNumberOfNodes() const; - VecTimes dts_; ///< times at which the constraint is evaluated. + VecTimes dts_; ///< times at which the constraint is evaluated. -private: + private: /** * @brief Sets the constraint value a specific time t, corresponding to node k. * @param t The time along the trajectory to set the constraint. @@ -84,7 +85,8 @@ class TimeDiscretizationConstraint : public ifopt::ConstraintSet { * @param[in/out] g The complete vector of constraint values, for which the * corresponding row must be filled. */ - virtual void UpdateConstraintAtInstance(double t, int k, VectorXd& g) const = 0; + virtual void UpdateConstraintAtInstance(double t, int k, + VectorXd& g) const = 0; /** * @brief Sets upper/lower bound a specific time t, corresponding to node k. diff --git a/towr/include/towr/constraints/total_duration_constraint.h b/towr/include/towr/constraints/total_duration_constraint.h index 31c716302..3a0a1495c 100644 --- a/towr/include/towr/constraints/total_duration_constraint.h +++ b/towr/include/towr/constraints/total_duration_constraint.h @@ -50,7 +50,7 @@ namespace towr { * @ingroup Constraints */ class TotalDurationConstraint : public ifopt::ConstraintSet { -public: + public: using EE = uint; TotalDurationConstraint(double T_total, int ee); @@ -60,14 +60,14 @@ class TotalDurationConstraint : public ifopt::ConstraintSet { VectorXd GetValues() const override; VecBound GetBounds() const override; - void FillJacobianBlock (std::string var_set, Jacobian&) const override; + void FillJacobianBlock(std::string var_set, Jacobian&) const override; -private: + private: PhaseDurations::Ptr phase_durations_; double T_total_; EE ee_; }; -} // namespace towr +} // namespace towr #endif /* TOWR_CONSTRAINTS_TOTAL_DURATION_CONSTRAINT_H_ */ diff --git a/towr/include/towr/costs/node_cost.h b/towr/include/towr/costs/node_cost.h index aae302615..f0efb213c 100644 --- a/towr/include/towr/costs/node_cost.h +++ b/towr/include/towr/costs/node_cost.h @@ -37,7 +37,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include - namespace towr { /** @@ -46,21 +45,21 @@ namespace towr { * @ingroup Costs */ class NodeCost : public ifopt::CostTerm { -public: + public: /** * @brief Constructs a cost term for the optimization problem. * @param nodes_id The name of the node variables. * @param deriv The node derivative (pos, vel) which should be penalized. * @param dim The node dimension which should be penalized. */ - NodeCost (const std::string& nodes_id, Dx deriv, int dim, double weight); - virtual ~NodeCost () = default; + NodeCost(const std::string& nodes_id, Dx deriv, int dim, double weight); + virtual ~NodeCost() = default; void InitVariableDependedQuantities(const VariablesPtr& x) override; - double GetCost () const override; + double GetCost() const override; -private: + private: std::shared_ptr nodes_; std::string node_id_; diff --git a/towr/include/towr/costs/soft_constraint.h b/towr/include/towr/costs/soft_constraint.h index 06fa26d13..99baec2b9 100644 --- a/towr/include/towr/costs/soft_constraint.h +++ b/towr/include/towr/costs/soft_constraint.h @@ -57,7 +57,7 @@ namespace towr { * @ingroup Costs */ class SoftConstraint : public ifopt::Component { -public: + public: using ConstraintPtr = Component::Ptr; /** @@ -67,20 +67,20 @@ class SoftConstraint : public ifopt::Component { * Weights are set to identity, so each constraint violation contributes * equally to the cost. */ - SoftConstraint (const ConstraintPtr& constraint); - virtual ~SoftConstraint () = default; + SoftConstraint(const ConstraintPtr& constraint); + virtual ~SoftConstraint() = default; -private: + private: ConstraintPtr constraint_; - VectorXd W_; ///< weights how each constraint violation contributes to the cost. - VectorXd b_; /// average value of each upper and lower bound. + VectorXd W_; ///< weight each constraint violation contributes to the cost. + VectorXd b_; /// average value of each upper and lower bound. /** * @brief The quadratic constraint violation transformed to a cost. * * c(x) = 0.5 * (g-b)^T * W * (g-b) */ - VectorXd GetValues () const override; + VectorXd GetValues() const override; /** * @brief The row-vector of derivatives of the cost term. @@ -90,7 +90,10 @@ class SoftConstraint : public ifopt::Component { Jacobian GetJacobian() const override; // doesn't exist for cost, generated run-time error when used - VecBound GetBounds() const final { return VecBound(GetRows(), ifopt::NoBound); }; + VecBound GetBounds() const final + { + return VecBound(GetRows(), ifopt::NoBound); + }; void SetVariables(const VectorXd& x) final { assert(false); }; }; diff --git a/towr/include/towr/initialization/biped_gait_generator.h b/towr/include/towr/initialization/biped_gait_generator.h index 11fcce435..7b9753184 100644 --- a/towr/include/towr/initialization/biped_gait_generator.h +++ b/towr/include/towr/initialization/biped_gait_generator.h @@ -40,11 +40,11 @@ namespace towr { * @sa GaitGenerator for more documentation */ class BipedGaitGenerator : public GaitGenerator { -public: - BipedGaitGenerator (); - virtual ~BipedGaitGenerator () = default; + public: + BipedGaitGenerator(); + virtual ~BipedGaitGenerator() = default; -private: + private: GaitInfo GetGait(Gaits gait) const override; GaitInfo GetStrideStand() const; @@ -59,10 +59,10 @@ class BipedGaitGenerator : public GaitGenerator { void SetCombo(Combos combo) override; // naming convention:, where the circle is is contact, front is right ->. - ContactState I_; // flight - ContactState b_; // right-leg in contact - ContactState P_; // left leg in contact - ContactState B_; // stance (both legs in contact) + ContactState I_; // flight + ContactState b_; // right-leg in contact + ContactState P_; // left leg in contact + ContactState B_; // stance (both legs in contact) }; } /* namespace towr */ diff --git a/towr/include/towr/initialization/gait_generator.h b/towr/include/towr/initialization/gait_generator.h index 6dfd5f019..f1d6002e8 100644 --- a/towr/include/towr/initialization/gait_generator.h +++ b/towr/include/towr/initialization/gait_generator.h @@ -44,32 +44,48 @@ namespace towr { * initialize the towr optimization problem. */ class GaitGenerator { -public: + public: using Ptr = std::shared_ptr; using VecTimes = std::vector; using FootDurations = std::vector; using ContactState = std::vector; - using GaitInfo = std::pair>; + using GaitInfo = std::pair>; using EE = uint; /** * @brief Predefined combinations of different strides. */ - enum Combos { C0, C1, C2, C3, C4, COMBO_COUNT}; + enum Combos { C0, C1, C2, C3, C4, COMBO_COUNT }; /** * @brief Predefined strides, each with a different gait diagram. */ - enum Gaits {Stand=0, Flight, - Walk1, Walk2, Walk2E, - Run2, Run2E, Run1, Run1E, Run3, Run3E, - Hop1, Hop1E, Hop2, Hop3, Hop3E, Hop5, Hop5E, - GAIT_COUNT}; + enum Gaits { + Stand = 0, + Flight, + Walk1, + Walk2, + Walk2E, + Run2, + Run2E, + Run1, + Run1E, + Run3, + Run3E, + Hop1, + Hop1E, + Hop2, + Hop3, + Hop3E, + Hop5, + Hop5E, + GAIT_COUNT + }; static Ptr MakeGaitGenerator(int leg_count); - GaitGenerator () = default; - virtual ~GaitGenerator () = default; + GaitGenerator() = default; + virtual ~GaitGenerator() = default; /** * @returns the swing and stance durations for the set gait. @@ -98,7 +114,7 @@ class GaitGenerator { */ void SetGaits(const std::vector& gaits); -protected: + protected: /// Phase times for the complete robot during which no contact state changes. std::vector times_; @@ -114,7 +130,7 @@ class GaitGenerator { */ GaitInfo RemoveTransition(const GaitInfo& g) const; -private: + private: FootDurations GetPhaseDurations() const; virtual GaitInfo GetGait(Gaits gait) const = 0; VecTimes GetNormalizedPhaseDurations(EE ee) const; diff --git a/towr/include/towr/initialization/monoped_gait_generator.h b/towr/include/towr/initialization/monoped_gait_generator.h index 6643d09de..1e48d85c6 100644 --- a/towr/include/towr/initialization/monoped_gait_generator.h +++ b/towr/include/towr/initialization/monoped_gait_generator.h @@ -40,20 +40,20 @@ namespace towr { * @sa GaitGenerator for more documentation */ class MonopedGaitGenerator : public GaitGenerator { -public: - MonopedGaitGenerator () = default; - virtual ~MonopedGaitGenerator () = default; + public: + MonopedGaitGenerator() = default; + virtual ~MonopedGaitGenerator() = default; -private: + private: GaitInfo GetGait(Gaits gait) const override; - GaitInfo GetStrideStand() const; - GaitInfo GetStrideFlight() const; - GaitInfo GetStrideHop() const; + GaitInfo GetStrideStand() const; + GaitInfo GetStrideFlight() const; + GaitInfo GetStrideHop() const; GaitInfo GetStrideHopLong() const; - ContactState o_ = ContactState(1, true); // stance - ContactState x_ = ContactState(1, false); // flight + ContactState o_ = ContactState(1, true); // stance + ContactState x_ = ContactState(1, false); // flight void SetCombo(Combos combo) override; }; diff --git a/towr/include/towr/initialization/quadruped_gait_generator.h b/towr/include/towr/initialization/quadruped_gait_generator.h index d962efc65..8b3c03e49 100644 --- a/towr/include/towr/initialization/quadruped_gait_generator.h +++ b/towr/include/towr/initialization/quadruped_gait_generator.h @@ -40,11 +40,11 @@ namespace towr { * @sa GaitGenerator for more documentation */ class QuadrupedGaitGenerator : public GaitGenerator { -public: - QuadrupedGaitGenerator (); - virtual ~QuadrupedGaitGenerator () = default; + public: + QuadrupedGaitGenerator(); + virtual ~QuadrupedGaitGenerator() = default; -private: + private: GaitInfo GetGait(Gaits gait) const override; GaitInfo GetStrideStand() const; @@ -53,36 +53,35 @@ class QuadrupedGaitGenerator : public GaitGenerator { GaitInfo GetStrideWalkOverlap() const; GaitInfo GetStrideTrot() const; GaitInfo GetStrideTrotFly() const; - GaitInfo GetStrideTrotFlyEnd () const; + GaitInfo GetStrideTrotFlyEnd() const; GaitInfo GetStridePace() const; GaitInfo GetStridePaceEnd() const; GaitInfo GetStrideBound() const; - GaitInfo GetStrideBoundEnd () const; + GaitInfo GetStrideBoundEnd() const; GaitInfo GetStrideGallop() const; GaitInfo GetStridePronk() const; GaitInfo GetStrideLimp() const; void SetCombo(Combos combo) override; - // naming convention:, where the circle is is contact, front is right ->. // so RF and LH in contact is (Pb): o . // . o // flight-phase ContactState II_; - // 1 swingleg + // 1 swing leg ContactState PI_; ContactState bI_; ContactState IP_; ContactState Ib_; - // 2 swinglegs + // 2 swing legs ContactState Pb_; ContactState bP_; ContactState BI_; ContactState IB_; ContactState PP_; ContactState bb_; - // 3 swinglegs + // 3 swing legs ContactState Bb_; ContactState BP_; ContactState bB_; diff --git a/towr/include/towr/models/dynamic_model.h b/towr/include/towr/models/dynamic_model.h index 006a2b6ef..f109214cf 100644 --- a/towr/include/towr/models/dynamic_model.h +++ b/towr/include/towr/models/dynamic_model.h @@ -64,13 +64,13 @@ namespace towr { * @ingroup Robots */ class DynamicModel { -public: + public: using Ptr = std::shared_ptr; using Vector3d = Eigen::Vector3d; using Matrix3d = Eigen::Matrix3d; using ComPos = Eigen::Vector3d; using AngVel = Eigen::Vector3d; - using BaseAcc = Eigen::Matrix; + using BaseAcc = Eigen::Matrix; using Jac = Eigen::SparseMatrix; using EEPos = std::vector; using EELoad = EEPos; @@ -87,8 +87,9 @@ class DynamicModel { * @param pos_W Position of each foot expressed in world frame */ void SetCurrent(const ComPos& com_W, const Vector3d com_acc_W, - const Matrix3d& w_R_b, const AngVel& omega_W, const Vector3d& omega_dot_W, - const EELoad& force_W, const EEPos& pos_W); + const Matrix3d& w_R_b, const AngVel& omega_W, + const Vector3d& omega_dot_W, const EELoad& force_W, + const EEPos& pos_W); /** * @brief The violation of the system dynamics incurred by the current values. @@ -153,27 +154,27 @@ class DynamicModel { */ int GetEECount() const { return ee_pos_.size(); }; -protected: - ComPos com_pos_; ///< x-y-z position of the Center-of-Mass. - Vector3d com_acc_; ///< x-y-z acceleration of the Center-of-Mass. + protected: + ComPos com_pos_; ///< x-y-z position of the Center-of-Mass. + Vector3d com_acc_; ///< x-y-z acceleration of the Center-of-Mass. - Matrix3d w_R_b_; ///< rotation matrix from base (b) to world (w) frame. - AngVel omega_; ///< angular velocity expressed in world frame. - Vector3d omega_dot_; ///< angular acceleration expressed in world frame. + Matrix3d w_R_b_; ///< rotation matrix from base (b) to world (w) frame. + AngVel omega_; ///< angular velocity expressed in world frame. + Vector3d omega_dot_; ///< angular acceleration expressed in world frame. - EEPos ee_pos_; ///< The x-y-z position of each endeffector. - EELoad ee_force_; ///< The endeffector force expressed in world frame. + EEPos ee_pos_; ///< The x-y-z position of each endeffector. + EELoad ee_force_; ///< The endeffector force expressed in world frame. /** * @brief Construct a dynamic object. Protected as this is abstract base class. * @param mass The mass of the system. */ DynamicModel(double mass, int ee_count); - virtual ~DynamicModel () = default; + virtual ~DynamicModel() = default; -private: - double g_; ///< gravity acceleration [m/s^2] - double m_; ///< mass of the robot + private: + double g_; ///< gravity acceleration [m/s^2] + double m_; ///< mass of the robot }; } /* namespace towr */ diff --git a/towr/include/towr/models/endeffector_mappings.h b/towr/include/towr/models/endeffector_mappings.h index 7c3ce0abd..f463019b7 100644 --- a/towr/include/towr/models/endeffector_mappings.h +++ b/towr/include/towr/models/endeffector_mappings.h @@ -43,6 +43,6 @@ namespace towr { enum BipedIDs { L, R }; enum QuadrupedIDs { LF, RF, LH, RH }; -} // namespace towr +} // namespace towr #endif /* TOWR_STATES_ENDEFFECTOR_MAPPINGS_H_ */ diff --git a/towr/include/towr/models/examples/anymal_model.h b/towr/include/towr/models/examples/anymal_model.h index c8e1ba764..41c74f9f9 100644 --- a/towr/include/towr/models/examples/anymal_model.h +++ b/towr/include/towr/models/examples/anymal_model.h @@ -30,9 +30,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_ANYMAL_MODEL_H_ #define TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_ANYMAL_MODEL_H_ +#include #include #include -#include namespace towr { @@ -40,17 +40,17 @@ namespace towr { * @brief The Kinematics of the quadruped robot ANYmal. */ class AnymalKinematicModel : public KinematicModel { -public: - AnymalKinematicModel () : KinematicModel(4) + public: + AnymalKinematicModel() : KinematicModel(4) { const double x_nominal_b = 0.34; const double y_nominal_b = 0.19; const double z_nominal_b = -0.42; - nominal_stance_.at(LF) << x_nominal_b, y_nominal_b, z_nominal_b; - nominal_stance_.at(RF) << x_nominal_b, -y_nominal_b, z_nominal_b; - nominal_stance_.at(LH) << -x_nominal_b, y_nominal_b, z_nominal_b; - nominal_stance_.at(RH) << -x_nominal_b, -y_nominal_b, z_nominal_b; + nominal_stance_.at(LF) << x_nominal_b, y_nominal_b, z_nominal_b; + nominal_stance_.at(RF) << x_nominal_b, -y_nominal_b, z_nominal_b; + nominal_stance_.at(LH) << -x_nominal_b, y_nominal_b, z_nominal_b; + nominal_stance_.at(RH) << -x_nominal_b, -y_nominal_b, z_nominal_b; max_dev_from_nominal_ << 0.15, 0.1, 0.10; } @@ -60,13 +60,13 @@ class AnymalKinematicModel : public KinematicModel { * @brief The Dynamics of the quadruped robot ANYmal. */ class AnymalDynamicModel : public SingleRigidBodyDynamics { -public: + public: AnymalDynamicModel() - : SingleRigidBodyDynamics(29.5, - 0.946438, 1.94478, 2.01835, 0.000938112, -0.00595386, -0.00146328, - 4) {} + : SingleRigidBodyDynamics(29.5, 0.946438, 1.94478, 2.01835, 0.000938112, + -0.00595386, -0.00146328, 4) + {} }; -} // namespace towr +} // namespace towr #endif /* TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_ANYMAL_MODEL_H_ */ diff --git a/towr/include/towr/models/examples/biped_model.h b/towr/include/towr/models/examples/biped_model.h index cfd39a42b..aa90196c2 100644 --- a/towr/include/towr/models/examples/biped_model.h +++ b/towr/include/towr/models/examples/biped_model.h @@ -30,9 +30,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_BIPED_MODEL_H_ #define TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_BIPED_MODEL_H_ +#include #include #include -#include namespace towr { @@ -40,16 +40,16 @@ namespace towr { * @brief The Kinematics of a tow-legged robot built from HyQ legs. */ class BipedKinematicModel : public KinematicModel { -public: - BipedKinematicModel () : KinematicModel(2) + public: + BipedKinematicModel() : KinematicModel(2) { const double z_nominal_b = -0.65; - const double y_nominal_b = 0.20; + const double y_nominal_b = 0.20; - nominal_stance_.at(L) << 0.0, y_nominal_b, z_nominal_b; + nominal_stance_.at(L) << 0.0, y_nominal_b, z_nominal_b; nominal_stance_.at(R) << 0.0, -y_nominal_b, z_nominal_b; - max_dev_from_nominal_ << 0.25, 0.15, 0.15; + max_dev_from_nominal_ << 0.25, 0.15, 0.15; } }; @@ -57,11 +57,11 @@ class BipedKinematicModel : public KinematicModel { * @brief The Dynamics of a tow-legged robot built from HyQ legs. */ class BipedDynamicModel : public SingleRigidBodyDynamics { -public: + public: BipedDynamicModel() - : SingleRigidBodyDynamics(20, - 1.209,5.583,6.056,0.005,-0.190,-0.012, - 2) {} + : SingleRigidBodyDynamics(20, 1.209, 5.583, 6.056, 0.005, -0.190, -0.012, + 2) + {} }; } /* namespace towr */ diff --git a/towr/include/towr/models/examples/hyq_model.h b/towr/include/towr/models/examples/hyq_model.h index f05ee4158..793dc28d2 100644 --- a/towr/include/towr/models/examples/hyq_model.h +++ b/towr/include/towr/models/examples/hyq_model.h @@ -30,9 +30,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_HYQ_MODEL_H_ #define TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_HYQ_MODEL_H_ +#include #include #include -#include namespace towr { @@ -40,17 +40,17 @@ namespace towr { * @brief The Kinematics of the quadruped robot HyQ. */ class HyqKinematicModel : public KinematicModel { -public: - HyqKinematicModel () : KinematicModel(4) + public: + HyqKinematicModel() : KinematicModel(4) { const double x_nominal_b = 0.31; const double y_nominal_b = 0.29; const double z_nominal_b = -0.58; - nominal_stance_.at(LF) << x_nominal_b, y_nominal_b, z_nominal_b; - nominal_stance_.at(RF) << x_nominal_b, -y_nominal_b, z_nominal_b; - nominal_stance_.at(LH) << -x_nominal_b, y_nominal_b, z_nominal_b; - nominal_stance_.at(RH) << -x_nominal_b, -y_nominal_b, z_nominal_b; + nominal_stance_.at(LF) << x_nominal_b, y_nominal_b, z_nominal_b; + nominal_stance_.at(RF) << x_nominal_b, -y_nominal_b, z_nominal_b; + nominal_stance_.at(LH) << -x_nominal_b, y_nominal_b, z_nominal_b; + nominal_stance_.at(RH) << -x_nominal_b, -y_nominal_b, z_nominal_b; max_dev_from_nominal_ << 0.25, 0.20, 0.10; } @@ -60,10 +60,10 @@ class HyqKinematicModel : public KinematicModel { * @brief The Dynamics of the quadruped robot HyQ. */ class HyqDynamicModel : public SingleRigidBodyDynamics { -public: - HyqDynamicModel() : SingleRigidBodyDynamics(83, - 4.26, 8.97, 9.88, -0.0063, 0.193, 0.0126, - 4) {} + public: + HyqDynamicModel() + : SingleRigidBodyDynamics(83, 4.26, 8.97, 9.88, -0.0063, 0.193, 0.0126, 4) + {} }; } /* namespace towr */ diff --git a/towr/include/towr/models/examples/monoped_model.h b/towr/include/towr/models/examples/monoped_model.h index 844439f55..3295cfed1 100644 --- a/towr/include/towr/models/examples/monoped_model.h +++ b/towr/include/towr/models/examples/monoped_model.h @@ -39,10 +39,10 @@ namespace towr { * @brief The Kinematics of a one-legged hopper with HyQ leg. */ class MonopedKinematicModel : public KinematicModel { -public: - MonopedKinematicModel () : KinematicModel(1) + public: + MonopedKinematicModel() : KinematicModel(1) { - nominal_stance_.at(0) = Eigen::Vector3d( 0.0, 0.0, -0.58); + nominal_stance_.at(0) = Eigen::Vector3d(0.0, 0.0, -0.58); max_dev_from_nominal_ << 0.25, 0.15, 0.2; } }; @@ -51,11 +51,13 @@ class MonopedKinematicModel : public KinematicModel { * @brief The Dynamics of a one-legged hopper with HyQ leg. */ class MonopedDynamicModel : public SingleRigidBodyDynamics { -public: + public: + // clang-format off MonopedDynamicModel() - : SingleRigidBodyDynamics(20, // mass of the robot - 1.2, 5.5, 6.0, 0.0, -0.2, -0.01, // base inertia - 1) {} // number of endeffectors + : SingleRigidBodyDynamics(20, // mass of the robot + 1.2, 5.5, 6.0, 0.0, -0.2, -0.01, // base inertia + 1) {} // number of endeffectors + // clang-format on }; } /* namespace towr */ diff --git a/towr/include/towr/models/kinematic_model.h b/towr/include/towr/models/kinematic_model.h index b75e6d715..889c98b64 100644 --- a/towr/include/towr/models/kinematic_model.h +++ b/towr/include/towr/models/kinematic_model.h @@ -46,7 +46,7 @@ namespace towr { * @ingroup Robots */ class KinematicModel { -public: + public: using Ptr = std::shared_ptr; using EEPos = std::vector; using Vector3d = Eigen::Vector3d; @@ -55,22 +55,19 @@ class KinematicModel { * @brief Constructs a kinematic model of a robot with zero range of motion. * @param n_ee The number of endeffectors of the robot. */ - KinematicModel (int n_ee) + KinematicModel(int n_ee) { nominal_stance_.resize(n_ee); max_dev_from_nominal_.setZero(); } - virtual ~KinematicModel () = default; + virtual ~KinematicModel() = default; /** * @brief The xyz-position [m] of each foot in default stance. * @returns The vector from base to each foot expressed in the base frame. */ - virtual EEPos GetNominalStanceInBase() const - { - return nominal_stance_; - } + virtual EEPos GetNominalStanceInBase() const { return nominal_stance_; } /** * @brief How far each foot can deviate from its nominal position. @@ -84,12 +81,9 @@ class KinematicModel { /** * @returns returns the number of endeffectors of this robot. */ - int GetNumberOfEndeffectors() const - { - return nominal_stance_.size(); - } + int GetNumberOfEndeffectors() const { return nominal_stance_.size(); } -protected: + protected: EEPos nominal_stance_; Vector3d max_dev_from_nominal_; }; diff --git a/towr/include/towr/models/robot_model.h b/towr/include/towr/models/robot_model.h index 102ca74ae..7c43cd9f6 100644 --- a/towr/include/towr/models/robot_model.h +++ b/towr/include/towr/models/robot_model.h @@ -67,28 +67,26 @@ struct RobotModel { * See folder: \ref include/towr/models/examples for more information. * @ingroup Robots */ - enum Robot { Monoped, ///< one-legged hopper - Biped, ///< two-legged - Hyq, ///< four-legged robot from IIT - Anymal, ///< four-legged robot from Anybotics - ROBOT_COUNT }; - + enum Robot { + Monoped, ///< one-legged hopper + Biped, ///< two-legged + Hyq, ///< four-legged robot from IIT + Anymal, ///< four-legged robot from Anybotics + ROBOT_COUNT + }; RobotModel() = default; RobotModel(Robot robot); KinematicModel::Ptr kinematic_model_; - DynamicModel::Ptr dynamic_model_; + DynamicModel::Ptr dynamic_model_; }; - -const static std::map robot_names = -{ - {RobotModel::Monoped, "Monoped"}, - {RobotModel::Biped, "Biped"}, - {RobotModel::Hyq, "Hyq"}, - {RobotModel::Anymal, "Anymal"} -}; +const static std::map robot_names = { + {RobotModel::Monoped, "Monoped"}, + {RobotModel::Biped, "Biped"}, + {RobotModel::Hyq, "Hyq"}, + {RobotModel::Anymal, "Anymal"}}; } /* namespace towr */ diff --git a/towr/include/towr/models/single_rigid_body_dynamics.h b/towr/include/towr/models/single_rigid_body_dynamics.h index 168188a76..ccc890486 100644 --- a/towr/include/towr/models/single_rigid_body_dynamics.h +++ b/towr/include/towr/models/single_rigid_body_dynamics.h @@ -54,7 +54,7 @@ namespace towr { * @ingroup Robots */ class SingleRigidBodyDynamics : public DynamicModel { -public: + public: /** * @brief Constructs a specific model. * @param mass The mass of the robot. @@ -63,7 +63,8 @@ class SingleRigidBodyDynamics : public DynamicModel { * This matrix maps angular accelerations expressed in * base frame to moments in base frame. */ - SingleRigidBodyDynamics (double mass, const Eigen::Matrix3d& inertia_b, int ee_count); + SingleRigidBodyDynamics(double mass, const Eigen::Matrix3d& inertia_b, + int ee_count); /** * @brief Constructs a specific model. @@ -71,12 +72,10 @@ class SingleRigidBodyDynamics : public DynamicModel { * @param I.. Elements of the 3x3 Inertia matrix * @param ee_count Number of endeffectors/forces. */ - SingleRigidBodyDynamics (double mass, - double Ixx, double Iyy, double Izz, - double Ixy, double Ixz, double Iyz, - int ee_count); + SingleRigidBodyDynamics(double mass, double Ixx, double Iyy, double Izz, + double Ixy, double Ixz, double Iyz, int ee_count); - virtual ~SingleRigidBodyDynamics () = default; + virtual ~SingleRigidBodyDynamics() = default; BaseAcc GetDynamicViolation() const override; @@ -88,14 +87,13 @@ class SingleRigidBodyDynamics : public DynamicModel { Jac GetJacobianWrtEEPos(const Jac& jac_ee_pos, EE) const override; -private: + private: /** Inertia of entire robot around the CoM expressed in a frame anchored * in the base. */ Eigen::SparseMatrix I_b; }; - } /* namespace towr */ #endif /* TOWR_MODELS_SINGLE_RIBID_BODY_DYNAMICS_MODEL_H_ */ diff --git a/towr/include/towr/nlp_formulation.h b/towr/include/towr/nlp_formulation.h index 47a00a7c7..1343e12ba 100644 --- a/towr/include/towr/nlp_formulation.h +++ b/towr/include/towr/nlp_formulation.h @@ -30,14 +30,14 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_NLP_FACTORY_H_ #define TOWR_NLP_FACTORY_H_ -#include #include #include +#include -#include #include -#include #include +#include +#include namespace towr { @@ -71,15 +71,15 @@ namespace towr { * in this paper: https://ieeexplore.ieee.org/document/8283570/ */ class NlpFormulation { -public: - using VariablePtrVec = std::vector; - using ContraintPtrVec = std::vector; - using CostPtrVec = std::vector; - using EEPos = std::vector; - using Vector3d = Eigen::Vector3d; + public: + using VariablePtrVec = std::vector; + using ContraintPtrVec = std::vector; + using CostPtrVec = std::vector; + using EEPos = std::vector; + using Vector3d = Eigen::Vector3d; - NlpFormulation (); - virtual ~NlpFormulation () = default; + NlpFormulation(); + virtual ~NlpFormulation() = default; /** * @brief The ifopt variable sets that will be optimized over. @@ -96,15 +96,14 @@ class NlpFormulation { /** @brief The ifopt costs to tune the motion. */ ContraintPtrVec GetCosts() const; - BaseState initial_base_; BaseState final_base_; - EEPos initial_ee_W_; + EEPos initial_ee_W_; RobotModel model_; HeightMap::Ptr terrain_; Parameters params_; -private: + private: // variables std::vector MakeBaseVariables() const; std::vector MakeEndeffectorVariables() const; diff --git a/towr/include/towr/parameters.h b/towr/include/towr/parameters.h index 4fa56ee94..b901997e0 100644 --- a/towr/include/towr/parameters.h +++ b/towr/include/towr/parameters.h @@ -30,9 +30,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_OPTIMIZATION_PARAMETERS_H_ #define TOWR_OPTIMIZATION_PARAMETERS_H_ -#include #include -#include // std::pair, std::make_pair +#include // std::pair, std::make_pair +#include namespace towr { @@ -131,32 +131,34 @@ namespace towr { * @ingroup Parameters */ class Parameters { -public: + public: /** * @brief Identifiers to be used to add certain constraints to the * optimization problem. */ - enum ConstraintName { Dynamic, ///< sets DynamicConstraint - EndeffectorRom, ///< sets RangeOfMotionConstraint - TotalTime, ///< sets TotalDurationConstraint - Terrain, ///< sets TerrainConstraint - Force, ///< sets ForceConstraint - Swing, ///< sets SwingConstraint - BaseRom, ///< sets BaseMotionConstraint - BaseAcc ///< sets SplineAccConstraint + enum ConstraintName { + Dynamic, ///< sets DynamicConstraint + EndeffectorRom, ///< sets RangeOfMotionConstraint + TotalTime, ///< sets TotalDurationConstraint + Terrain, ///< sets TerrainConstraint + Force, ///< sets ForceConstraint + Swing, ///< sets SwingConstraint + BaseRom, ///< sets BaseMotionConstraint + BaseAcc ///< sets SplineAccConstraint }; /** * @brief Indentifiers to be used to add certain costs to the optimization * problem. */ - enum CostName { ForcesCostID, ///< sets NodeCost on force nodes - EEMotionCostID ///< sets NodeCost on endeffector velocity + enum CostName { + ForcesCostID, ///< sets NodeCost on force nodes + EEMotionCostID ///< sets NodeCost on endeffector velocity }; - using CostWeights = std::vector>; - using UsedConstraints = std::vector; - using VecTimes = std::vector; - using EEID = unsigned int; + using CostWeights = std::vector>; + using UsedConstraints = std::vector; + using VecTimes = std::vector; + using EEID = unsigned int; /** * @brief Default parameters to get started. @@ -198,10 +200,8 @@ class Parameters { double force_limit_in_normal_direction_; /// which dimensions (x,y,z) of the final base state should be bounded - std::vector bounds_final_lin_pos_, - bounds_final_lin_vel_, - bounds_final_ang_pos_, - bounds_final_ang_vel_; + std::vector bounds_final_lin_pos_, bounds_final_lin_vel_, + bounds_final_ang_pos_, bounds_final_ang_vel_; /** Minimum and maximum time [s] for each phase (swing,stance). * @@ -209,7 +209,7 @@ class Parameters { * Make sure max time is less than total duration of trajectory, or segfault. * limiting this range can help convergence when optimizing gait. */ - std::pair bound_phase_duration_; + std::pair bound_phase_duration_; /// Specifies that timings of all feet, so the gait, should be optimized. void OptimizePhaseDurations(); @@ -230,6 +230,6 @@ class Parameters { double GetTotalTime() const; }; -} // namespace towr +} // namespace towr #endif /* TOWR_OPTIMIZATION_PARAMETERS_H_ */ diff --git a/towr/include/towr/terrain/examples/height_map_examples.h b/towr/include/towr/terrain/examples/height_map_examples.h index ec576d96f..670696178 100644 --- a/towr/include/towr/terrain/examples/height_map_examples.h +++ b/towr/include/towr/terrain/examples/height_map_examples.h @@ -43,126 +43,126 @@ namespace towr { * @brief Sample terrain of even height. */ class FlatGround : public HeightMap { -public: + public: FlatGround(double height = 0.0); - double GetHeight(double x, double y) const override { return height_; }; + double GetHeight(double x, double y) const override { return height_; }; -private: - double height_; // [m] + private: + double height_; // [m] }; /** * @brief Sample terrain with a step in height in x-direction. */ class Block : public HeightMap { -public: - double GetHeight(double x, double y) const override; + public: + double GetHeight(double x, double y) const override; double GetHeightDerivWrtX(double x, double y) const override; -private: + private: double block_start = 0.7; double length_ = 3.5; - double height_ = 0.5; // [m] + double height_ = 0.5; // [m] - double eps_ = 0.03; // approximate as slope - const double slope_ = height_/eps_; + double eps_ = 0.03; // approximate as slope + const double slope_ = height_ / eps_; }; /** * @brief Sample terrain with a two-steps in height in x-direction. */ class Stairs : public HeightMap { -public: + public: double GetHeight(double x, double y) const override; -private: + private: double first_step_start_ = 1.0; double first_step_width_ = 0.4; double height_first_step = 0.2; double height_second_step = 0.4; - double width_top = 1.0; + double width_top = 1.0; }; /** * @brief Sample terrain with parabola-modeled gap in x-direction. */ class Gap : public HeightMap { -public: + public: double GetHeight(double x, double y) const override; double GetHeightDerivWrtX(double x, double y) const override; double GetHeightDerivWrtXX(double x, double y) const override; -private: + private: const double gap_start_ = 1.0; - const double w = 0.5; - const double h = 1.5; + const double w = 0.5; + const double h = 1.5; - const double slope_ = h/w; - const double dx = w/2.0; - const double xc = gap_start_ + dx; // gap center + const double slope_ = h / w; + const double dx = w / 2.0; + const double xc = gap_start_ + dx; // gap center const double gap_end_x = gap_start_ + w; // generated with matlab // see matlab/gap_height_map.m // coefficients of 2nd order polynomial // h = a*x^2 + b*x + c - const double a = (4*h)/(w*w); - const double b = -(8*h*xc)/(w*w); - const double c = -(h*(w - 2*xc)*(w + 2*xc))/(w*w); + const double a = (4 * h) / (w * w); + const double b = -(8 * h * xc) / (w * w); + const double c = -(h * (w - 2 * xc) * (w + 2 * xc)) / (w * w); }; /** * @brief Sample terrain with an increasing and then decreasing slope in x-direction. */ class Slope : public HeightMap { -public: + public: double GetHeight(double x, double y) const override; double GetHeightDerivWrtX(double x, double y) const override; -private: - const double slope_start_ = 1.0; - const double up_length_ = 1.0; - const double down_length_ = 1.0; + private: + const double slope_start_ = 1.0; + const double up_length_ = 1.0; + const double down_length_ = 1.0; const double height_center = 0.7; - const double x_down_start_ = slope_start_+up_length_; + const double x_down_start_ = slope_start_ + up_length_; const double x_flat_start_ = x_down_start_ + down_length_; - const double slope_ = height_center/up_length_; + const double slope_ = height_center / up_length_; }; /** * @brief Sample terrain with a tilted vertical wall to cross a gap. */ class Chimney : public HeightMap { -public: + public: double GetHeight(double x, double y) const override; double GetHeightDerivWrtY(double x, double y) const override; -private: + private: const double x_start_ = 1.0; const double length_ = 1.5; - const double y_start_ = 0.5; // distance to start of slope from center at z=0 + const double y_start_ = 0.5; // distance to start of slope from center at z=0 const double slope_ = 3.0; - const double x_end_ = x_start_+length_; + const double x_end_ = x_start_ + length_; }; /** * @brief Sample terrain with two tilted vertical walls to cross a gap. */ class ChimneyLR : public HeightMap { -public: + public: double GetHeight(double x, double y) const override; double GetHeightDerivWrtY(double x, double y) const override; -private: + private: const double x_start_ = 0.5; const double length_ = 1.0; - const double y_start_ = 0.5; // distance to start of slope from center at z=0 + const double y_start_ = 0.5; // distance to start of slope from center at z=0 const double slope_ = 2; - const double x_end1_ = x_start_+length_; - const double x_end2_ = x_start_+2*length_; + const double x_end1_ = x_start_ + length_; + const double x_end2_ = x_start_ + 2 * length_; }; /** @}*/ diff --git a/towr/include/towr/terrain/height_map.h b/towr/include/towr/terrain/height_map.h index ca65c37bf..8d0fcf16d 100644 --- a/towr/include/towr/terrain/height_map.h +++ b/towr/include/towr/terrain/height_map.h @@ -30,10 +30,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_HEIGHT_MAP_H_ #define TOWR_HEIGHT_MAP_H_ -#include -#include #include +#include #include +#include #include @@ -69,28 +69,30 @@ namespace towr { * @ingroup Terrains */ class HeightMap { -public: + public: using Ptr = std::shared_ptr; using Vector3d = Eigen::Vector3d; /** * @brief Terrains IDs corresponding for factory method. */ - enum TerrainID { FlatID, - BlockID, - StairsID, - GapID, - SlopeID, - ChimneyID, - ChimneyLRID, - TERRAIN_COUNT }; + enum TerrainID { + FlatID, + BlockID, + StairsID, + GapID, + SlopeID, + ChimneyID, + ChimneyLRID, + TERRAIN_COUNT + }; static HeightMap::Ptr MakeTerrain(TerrainID type); enum Direction { Normal, Tangent1, Tangent2 }; - HeightMap() = default; - virtual ~HeightMap () = default; + HeightMap() = default; + virtual ~HeightMap() = default; /** * @returns The height of the terrain [m] at a specific 2D position. @@ -132,11 +134,11 @@ class HeightMap { */ double GetFrictionCoeff() const { return friction_coeff_; }; -protected: + protected: double friction_coeff_ = 0.5; -private: - using DimDerivs = std::vector; ///< dimensional derivatives + private: + using DimDerivs = std::vector; ///< dimensional derivatives /** * @brief returns either the terrain normal/tangent or its derivative. * @param direction Terrain normal or tangent vector. @@ -147,14 +149,14 @@ class HeightMap { * @returns the 3D @b not-normalized vector. */ Vector3d GetBasis(Direction direction, double x, double y, - const DimDerivs& dim_deriv= {}) const; + const DimDerivs& dim_deriv = {}) const; - Vector3d GetNormal(double x, double y, const DimDerivs& = {}) const; + Vector3d GetNormal(double x, double y, const DimDerivs& = {}) const; Vector3d GetTangent1(double x, double y, const DimDerivs& = {}) const; Vector3d GetTangent2(double x, double y, const DimDerivs& = {}) const; - double GetSecondDerivativeOfHeightWrt(Dim2D dim1, Dim2D dim2, - double x, double y) const; + double GetSecondDerivativeOfHeightWrt(Dim2D dim1, Dim2D dim2, double x, + double y) const; Vector3d GetDerivativeOfNormalizedVectorWrtNonNormalizedIndex( const Vector3d& non_normalized, int index) const; @@ -170,17 +172,11 @@ class HeightMap { virtual double GetHeightDerivWrtYY(double x, double y) const { return 0.0; }; }; - -const static std::map terrain_names = -{ - {HeightMap::FlatID, "Flat" }, - {HeightMap::BlockID, "Block" }, - {HeightMap::StairsID, "Stairs" }, - {HeightMap::GapID, "Gap" }, - {HeightMap::SlopeID, "Slope" }, - {HeightMap::ChimneyID, "Chimney" }, - {HeightMap::ChimneyLRID, "ChimenyLR" } -}; +const static std::map terrain_names = { + {HeightMap::FlatID, "Flat"}, {HeightMap::BlockID, "Block"}, + {HeightMap::StairsID, "Stairs"}, {HeightMap::GapID, "Gap"}, + {HeightMap::SlopeID, "Slope"}, {HeightMap::ChimneyID, "Chimney"}, + {HeightMap::ChimneyLRID, "ChimenyLR"}}; } /* namespace towr */ diff --git a/towr/include/towr/variables/cartesian_dimensions.h b/towr/include/towr/variables/cartesian_dimensions.h index 74aebd98a..15db7f196 100644 --- a/towr/include/towr/variables/cartesian_dimensions.h +++ b/towr/include/towr/variables/cartesian_dimensions.h @@ -42,11 +42,11 @@ namespace towr { // 2-dimensional static constexpr int k2D = 2; -enum Dim2D { X_=0, Y_}; +enum Dim2D { X_ = 0, Y_ }; // 3-dimensional static constexpr int k3D = 3; -enum Dim3D { X=0, Y, Z }; +enum Dim3D { X = 0, Y, Z }; static Dim2D To2D(Dim3D dim) { assert(dim != Z); @@ -55,10 +55,10 @@ static Dim2D To2D(Dim3D dim) // 6-dimensional // 'A' stands for angular, 'L' for linear. -static constexpr int k6D = 6; // X,Y,Z, roll, pitch, yaw -enum Dim6D { AX=0, AY, AZ, LX, LY, LZ }; +static constexpr int k6D = 6; // X,Y,Z, roll, pitch, yaw +enum Dim6D { AX = 0, AY, AZ, LX, LY, LZ }; static const Dim6D AllDim6D[] = {AX, AY, AZ, LX, LY, LZ}; -} // namespace towr +} // namespace towr #endif /* TOWR_VARIABLES_CARTESIAN_DIMENSIONS_H_ */ diff --git a/towr/include/towr/variables/euler_converter.h b/towr/include/towr/variables/euler_converter.h index e4bdcaa08..ce3f31a46 100644 --- a/towr/include/towr/variables/euler_converter.h +++ b/towr/include/towr/variables/euler_converter.h @@ -60,17 +60,17 @@ namespace towr { * See matlab script "matlab/euler_converter.m" for derivation. */ class EulerConverter { -public: + public: using Vector3d = Eigen::Vector3d; - using EulerAngles = Vector3d; ///< roll, pitch, yaw. - using EulerRates = Vector3d; ///< derivative of the above + using EulerAngles = Vector3d; ///< roll, pitch, yaw. + using EulerRates = Vector3d; ///< derivative of the above - using JacobianRow = Eigen::SparseVector; - using MatrixSXd = Eigen::SparseMatrix; - using Jacobian = MatrixSXd; + using JacobianRow = Eigen::SparseVector; + using MatrixSXd = Eigen::SparseMatrix; + using Jacobian = MatrixSXd; using JacRowMatrix = std::array, k3D>; - EulerConverter () = default; + EulerConverter() = default; /** * @brief Constructs and links this object to the Euler angle values @@ -87,15 +87,15 @@ class EulerConverter { * double pitch = euler_angles->GetPoint(t).y(); * double yaw = euler_angles->GetPoint(t).z(); */ - EulerConverter (const NodeSpline::Ptr& euler_angles); - virtual ~EulerConverter () = default; + EulerConverter(const NodeSpline::Ptr& euler_angles); + virtual ~EulerConverter() = default; /** * @brief Converts the Euler angles at time t to a Quaternion. * @param t The current time in the euler angles spline. * @return A Quaternion the maps a vector from base to world frame. */ - Eigen::Quaterniond GetQuaternionBaseToWorld (double t) const; + Eigen::Quaterniond GetQuaternionBaseToWorld(double t) const; /** * @brief Converts the Euler angles at time t to a rotation matrix. @@ -155,7 +155,7 @@ class EulerConverter { /** @see GetQuaternionBaseToWorld(t) */ static Eigen::Quaterniond GetQuaternionBaseToWorld(const EulerAngles& pos); -private: + private: NodeSpline::Ptr euler_; // Internal calculations for the conversion from euler rates to angular diff --git a/towr/include/towr/variables/node_spline.h b/towr/include/towr/variables/node_spline.h index f6f4775aa..af90c20bd 100644 --- a/towr/include/towr/variables/node_spline.h +++ b/towr/include/towr/variables/node_spline.h @@ -30,11 +30,11 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_TOWR_SRC_NODE_SPLINE_H_ #define TOWR_TOWR_SRC_NODE_SPLINE_H_ -#include #include +#include -#include "spline.h" #include "nodes_observer.h" +#include "spline.h" namespace towr { @@ -47,8 +47,8 @@ namespace towr { * polynomials accordingly. */ class NodeSpline : public Spline, public NodesObserver { -public: - using Ptr = std::shared_ptr; + public: + using Ptr = std::shared_ptr; using Jacobian = Eigen::SparseMatrix; /** @@ -98,10 +98,12 @@ class NodeSpline : public Spline, public NodesObserver { * p: Number of dimensions of the spline * n: Number of optimized durations. */ - virtual Jacobian - GetJacobianOfPosWrtDurations(double t) const { assert(false); } // durations are fixed here + virtual Jacobian GetJacobianOfPosWrtDurations(double t) const + { + assert(false); + } // durations are fixed here -protected: + protected: /** * The size and non-zero elements of the Jacobian of the position w.r.t nodes. */ @@ -115,8 +117,8 @@ class NodeSpline : public Spline, public NodesObserver { * @param jac[in/out] The correctly sized Jacobian to fill. * @param fill_with_zeros True if only sparsity pattern should be set. */ - void FillJacobianWrtNodes (int poly_id, double t_local, Dx dxdt, - Jacobian& jac, bool fill_with_zeros) const; + void FillJacobianWrtNodes(int poly_id, double t_local, Dx dxdt, Jacobian& jac, + bool fill_with_zeros) const; }; } /* namespace towr */ diff --git a/towr/include/towr/variables/nodes_observer.h b/towr/include/towr/variables/nodes_observer.h index 545107516..cb8b5f510 100644 --- a/towr/include/towr/variables/nodes_observer.h +++ b/towr/include/towr/variables/nodes_observer.h @@ -30,7 +30,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_TOWR_INCLUDE_TOWR_VARIABLES_NODES_OBSERVER_H_ #define TOWR_TOWR_INCLUDE_TOWR_VARIABLES_NODES_OBSERVER_H_ - namespace towr { class NodesVariables; @@ -48,8 +47,8 @@ class NodesVariables; * https://sourcemaking.com/design_patterns/observer */ class NodesObserver { -public: - using NodeSubjectPtr = NodesVariables*; // observer shouldn't own subject + public: + using NodeSubjectPtr = NodesVariables*; // observer shouldn't own subject /** * @brief Registers this observer with the subject class to receive updates. @@ -63,7 +62,7 @@ class NodesObserver { */ virtual void UpdateNodes() = 0; -protected: + protected: NodeSubjectPtr node_values_; }; diff --git a/towr/include/towr/variables/nodes_variables.h b/towr/include/towr/variables/nodes_variables.h index fbdebe3c4..5439c8ae0 100644 --- a/towr/include/towr/variables/nodes_variables.h +++ b/towr/include/towr/variables/nodes_variables.h @@ -32,8 +32,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include "state.h" #include "nodes_observer.h" +#include "state.h" namespace towr { @@ -69,7 +69,7 @@ namespace towr { * @ingroup Variables */ class NodesVariables : public ifopt::VariableSet { -public: + public: using Ptr = std::shared_ptr; using VecDurations = std::vector; using ObserverPtr = NodesObserver*; @@ -83,9 +83,9 @@ class NodesVariables : public ifopt::VariableSet { * @sa GetNodeValuesInfo() */ struct NodeValueInfo { - int id_; ///< ID of the associated node (0 =< id < number of nodes in spline). - Dx deriv_; ///< Derivative (pos,vel) of the node with that ID. - int dim_; ///< Dimension (x,y,z) of that derivative. + int id_; ///< ID of the associated node (0 =< id < number of nodes in spline). + Dx deriv_; ///< Derivative (pos,vel) of the node with that ID. + int dim_; ///< Dimension (x,y,z) of that derivative. NodeValueInfo() = default; NodeValueInfo(int node_id, Dx deriv, int node_dim); @@ -122,7 +122,7 @@ class NodesVariables : public ifopt::VariableSet { * * @sa GetNodeInfoAtOptIndex() */ - VectorXd GetValues () const override; + VectorXd GetValues() const override; /** * @brief Sets some node positions and velocity from the optimization variables. @@ -134,12 +134,12 @@ class NodesVariables : public ifopt::VariableSet { * * @sa GetNodeValuesInfo() */ - void SetVariables (const VectorXd&x) override; + void SetVariables(const VectorXd& x) override; /** * @returns the bounds on position and velocity of each node and dimension. */ - VecBound GetBounds () const override; + VecBound GetBounds() const override; /** * @returns All the nodes that can be used to reconstruct the spline. @@ -156,7 +156,7 @@ class NodesVariables : public ifopt::VariableSet { */ const std::vector GetBoundaryNodes(int poly_id) const; - enum Side {Start=0, End}; + enum Side { Start = 0, End }; /** * @brief The node ID that belongs to a specific side of a specific polynomial. * @param poly_id The ID of the polynomial within the spline. @@ -182,8 +182,7 @@ class NodesVariables : public ifopt::VariableSet { * @param t_total The total duration to reach final node (to set velocities). */ void SetByLinearInterpolation(const VectorXd& initial_val, - const VectorXd& final_val, - double t_total); + const VectorXd& final_val, double t_total); /** * @brief Restricts the first node in the spline. @@ -191,8 +190,8 @@ class NodesVariables : public ifopt::VariableSet { * @param dimensions Which dimensions (x,y,z) should be restricted. * @param val The values the fist node should be set to. */ - void AddStartBound (Dx deriv, const std::vector& dimensions, - const VectorXd& val); + void AddStartBound(Dx deriv, const std::vector& dimensions, + const VectorXd& val); /** * @brief Restricts the last node in the spline. @@ -203,19 +202,19 @@ class NodesVariables : public ifopt::VariableSet { void AddFinalBound(Dx deriv, const std::vector& dimensions, const VectorXd& val); -protected: + protected: /** * @param n_dim The number of dimensions (x,y,..) each node has. * @param variable_name The name of the variables in the optimization problem. */ - NodesVariables (const std::string& variable_name); - virtual ~NodesVariables () = default; + NodesVariables(const std::string& variable_name); + virtual ~NodesVariables() = default; - VecBound bounds_; ///< the bounds on the node values. + VecBound bounds_; ///< the bounds on the node values. std::vector nodes_; int n_dim_; -private: + private: /** * @brief Notifies the subscribed observers that the node values changes. */ diff --git a/towr/include/towr/variables/nodes_variables_all.h b/towr/include/towr/variables/nodes_variables_all.h index 81329fd80..ccf35beb1 100644 --- a/towr/include/towr/variables/nodes_variables_all.h +++ b/towr/include/towr/variables/nodes_variables_all.h @@ -43,14 +43,14 @@ namespace towr { * @ingroup Variables */ class NodesVariablesAll : public NodesVariables { -public: + public: /** * @param n_nodes Number of nodes to construct the spline. * @param n_dim Number of dimensions of each node. * @param variable_id Name of this variables set in the optimization. */ - NodesVariablesAll (int n_nodes, int n_dim, std::string variable_id); - virtual ~NodesVariablesAll () = default; + NodesVariablesAll(int n_nodes, int n_dim, std::string variable_id); + virtual ~NodesVariablesAll() = default; std::vector GetNodeValuesInfo(int idx) const override; }; diff --git a/towr/include/towr/variables/nodes_variables_phase_based.h b/towr/include/towr/variables/nodes_variables_phase_based.h index 79e280130..8fd29eed4 100644 --- a/towr/include/towr/variables/nodes_variables_phase_based.h +++ b/towr/include/towr/variables/nodes_variables_phase_based.h @@ -57,19 +57,19 @@ namespace towr { * @ingroup Variables */ class NodesVariablesPhaseBased : public NodesVariables { -public: + public: using Ptr = std::shared_ptr; using NodeIds = std::vector; - using OptIndexMap = std::map >; + using OptIndexMap = std::map>; /** * @brief Holds semantic information each polynomial in spline. */ struct PolyInfo { - int phase_; ///< The phase ID this polynomial represents. - int poly_in_phase_; ///< is this the 1st, 2nd, ... polynomial or this phase. - int n_polys_in_phase_; ///< the number of polynomials used for this phase. - bool is_constant_; ///< Does this polynomial represent a constant phase. + int phase_; ///< The phase ID this polynomial represents. + int poly_in_phase_; ///< is this the 1st, 2nd, ... polynomial or this phase. + int n_polys_in_phase_; ///< the number of polynomials used for this phase. + bool is_constant_; ///< Does this polynomial represent a constant phase. PolyInfo(int phase, int poly_in_phase, int n_polys_in_phase, bool is_const); }; @@ -81,10 +81,9 @@ class NodesVariablesPhaseBased : public NodesVariables { * @param n_polys_in_changing_phase How many polynomials should be used to * paramerize each non-constant phase. */ - NodesVariablesPhaseBased (int phase_count, - bool first_phase_constant, - const std::string& var_name, - int n_polys_in_changing_phase); + NodesVariablesPhaseBased(int phase_count, bool first_phase_constant, + const std::string& var_name, + int n_polys_in_changing_phase); virtual ~NodesVariablesPhaseBased() = default; @@ -125,8 +124,8 @@ class NodesVariablesPhaseBased : public NodesVariables { * @return The durations of each polynomial, where multiple polynomials can * be used to represent one phase. */ - virtual VecDurations - ConvertPhaseToPolyDurations(const VecDurations& phase_durations) const; + virtual VecDurations ConvertPhaseToPolyDurations( + const VecDurations& phase_durations) const; /** * @brief How a change in the phase duration affects the polynomial duration. @@ -136,8 +135,8 @@ class NodesVariablesPhaseBased : public NodesVariables { * T_poly = 1/3 * T_phase. * The derivative of T_poly is then 1/3. */ - virtual double - GetDerivativeOfPolyDurationWrtPhaseDuration(int polynomial_id) const; + virtual double GetDerivativeOfPolyDurationWrtPhaseDuration( + int polynomial_id) const; /** * @brief How many polynomials in the current phase come before. @@ -147,8 +146,7 @@ class NodesVariablesPhaseBased : public NodesVariables { * polynomial corresponds to the third one in the phase, then 2 polynomials * come before it. */ - virtual int - GetNumberOfPrevPolynomialsInPhase(int polynomial_id) const; + virtual int GetNumberOfPrevPolynomialsInPhase(int polynomial_id) const; /** * @brief Is the polynomial constant, so not changing the value. @@ -156,7 +154,7 @@ class NodesVariablesPhaseBased : public NodesVariables { */ virtual bool IsInConstantPhase(int polynomial_id) const; -protected: + protected: /** * @brief Assign optimization variables to the correct node values. * @@ -165,13 +163,14 @@ class NodesVariablesPhaseBased : public NodesVariables { * derived Motion and Force classes. */ OptIndexMap index_to_node_value_info_; - std::vector GetNodeValuesInfo(int idx) const override { + std::vector GetNodeValuesInfo(int idx) const override + { return index_to_node_value_info_.at(idx); } void SetNumberOfVariables(int n_variables); -private: + private: /** @brief semantic information associated with each polynomial */ std::vector polynomial_info_; @@ -182,36 +181,31 @@ class NodesVariablesPhaseBased : public NodesVariables { std::vector GetAdjacentPolyIds(int node_id) const; }; - /** * @brief Variables fully defining the endeffector motion. * * @ingroup Variables */ class NodesVariablesEEMotion : public NodesVariablesPhaseBased { -public: - NodesVariablesEEMotion(int phase_count, - bool is_in_contact_at_start, + public: + NodesVariablesEEMotion(int phase_count, bool is_in_contact_at_start, const std::string& name, int n_polys_in_changing_phase); virtual ~NodesVariablesEEMotion() = default; - OptIndexMap GetPhaseBasedEEParameterization (); + OptIndexMap GetPhaseBasedEEParameterization(); }; - /** * @brief Variables fully defining the endeffector forces. * * @ingroup Variables */ class NodesVariablesEEForce : public NodesVariablesPhaseBased { -public: - NodesVariablesEEForce(int phase_count, - bool is_in_contact_at_start, - const std::string& name, - int n_polys_in_changing_phase); + public: + NodesVariablesEEForce(int phase_count, bool is_in_contact_at_start, + const std::string& name, int n_polys_in_changing_phase); virtual ~NodesVariablesEEForce() = default; - OptIndexMap GetPhaseBasedEEParameterization (); + OptIndexMap GetPhaseBasedEEParameterization(); }; } /* namespace towr */ diff --git a/towr/include/towr/variables/phase_durations.h b/towr/include/towr/variables/phase_durations.h index aa9d37404..7b296a7c7 100644 --- a/towr/include/towr/variables/phase_durations.h +++ b/towr/include/towr/variables/phase_durations.h @@ -48,12 +48,11 @@ namespace towr { * @ingroup Variables */ class PhaseDurations : public ifopt::VariableSet { -public: + public: using Ptr = std::shared_ptr; using VecDurations = std::vector; using EndeffectorID = uint; - /** * @brief Constructs a variable set for a specific endeffector * @param ee The endeffector ID which these durations apply to. @@ -61,12 +60,10 @@ class PhaseDurations : public ifopt::VariableSet { * @param min_phase_duration The minimum allowable time for one phase. * @param max_phase_duration The maximum allowable time for one phase. */ - PhaseDurations (EndeffectorID ee, - const VecDurations& initial_durations, - bool is_first_phase_in_contact, - double min_phase_duration, - double max_phase_duration); - virtual ~PhaseDurations () = default; + PhaseDurations(EndeffectorID ee, const VecDurations& initial_durations, + bool is_first_phase_in_contact, double min_phase_duration, + double max_phase_duration); + virtual ~PhaseDurations() = default; /** * @returns The durations (stance, swing, ...) for each phase of this foot. @@ -86,7 +83,7 @@ class PhaseDurations : public ifopt::VariableSet { /** * @returns The maximum and minimum time each phase is allowed to take. */ - VecBound GetBounds () const override; + VecBound GetBounds() const override; /** * @brief How a change in the phase durations affect the position of a spline. @@ -103,7 +100,8 @@ class PhaseDurations : public ifopt::VariableSet { * polynomials through the duration. This method quantifies the sensitivity * of the spline position on these durations. */ - Jacobian GetJacobianOfPos(int phase, const VectorXd& dx_dT, const VectorXd& xd) const; + Jacobian GetJacobianOfPos(int phase, const VectorXd& dx_dT, + const VectorXd& xd) const; /** * @brief Adds observer that is updated every time new variables are set. @@ -117,11 +115,11 @@ class PhaseDurations : public ifopt::VariableSet { */ bool IsContactPhase(double t) const; -private: + private: VecDurations durations_; double t_total_; - bool initial_contact_state_; ///< true if first phase in contact + bool initial_contact_state_; ///< true if first phase in contact ifopt::Bounds phase_duration_bounds_; std::vector observers_; diff --git a/towr/include/towr/variables/phase_durations_observer.h b/towr/include/towr/variables/phase_durations_observer.h index 414535d43..480dcde24 100644 --- a/towr/include/towr/variables/phase_durations_observer.h +++ b/towr/include/towr/variables/phase_durations_observer.h @@ -30,7 +30,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_TOWR_INCLUDE_TOWR_VARIABLES_PHASE_DURATIONS_OBSERVER_H_ #define TOWR_TOWR_INCLUDE_TOWR_VARIABLES_PHASE_DURATIONS_OBSERVER_H_ - namespace towr { class PhaseDurations; @@ -48,8 +47,9 @@ class PhaseDurations; * https://sourcemaking.com/design_patterns/observer */ class PhaseDurationsObserver { -public: - using PhaseDurationsSubjectPtr = PhaseDurations*; // observer shouldn't own subject + public: + // observer shouldn't own subject + using PhaseDurationsSubjectPtr = PhaseDurations*; PhaseDurationsObserver() = default; @@ -65,7 +65,7 @@ class PhaseDurationsObserver { */ virtual void UpdatePolynomialDurations() = 0; -protected: + protected: PhaseDurationsSubjectPtr phase_durations_; }; diff --git a/towr/include/towr/variables/phase_spline.h b/towr/include/towr/variables/phase_spline.h index 6b700c76c..e555c1f2c 100644 --- a/towr/include/towr/variables/phase_spline.h +++ b/towr/include/towr/variables/phase_spline.h @@ -31,8 +31,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #define TOWR_TOWR_INCLUDE_TOWR_VARIABLES_PHASE_SPLINE_H_ #include "node_spline.h" -#include "phase_durations_observer.h" #include "nodes_variables_phase_based.h" +#include "phase_durations_observer.h" namespace towr { @@ -44,9 +44,9 @@ namespace towr { * CubicHermitePolynomial. For this it observers whether one of the quantities * changed and then updates all the polynomials accordingly. */ -class PhaseSpline : public NodeSpline, public PhaseDurationsObserver{ -public: - using Ptr = std::shared_ptr; +class PhaseSpline : public NodeSpline, public PhaseDurationsObserver { + public: + using Ptr = std::shared_ptr; using VectorXd = Eigen::VectorXd; /** @@ -72,15 +72,16 @@ class PhaseSpline : public NodeSpline, public PhaseDurationsObserver{ */ Jacobian GetJacobianOfPosWrtDurations(double t) const override; -private: + private: /** * @brief How the position at time t changes with current phase duration. * @param t The global time along the spline. * @return How a duration change affects the x,y,z position. */ - Eigen::VectorXd GetDerivativeOfPosWrtPhaseDuration (double t) const; + Eigen::VectorXd GetDerivativeOfPosWrtPhaseDuration(double t) const; - NodesVariablesPhaseBased::Ptr phase_nodes_; // retain pointer for extended functionality + // retain pointer for extended functionality + NodesVariablesPhaseBased::Ptr phase_nodes_; }; } /* namespace towr */ diff --git a/towr/include/towr/variables/polynomial.h b/towr/include/towr/variables/polynomial.h index edf3ec99d..3e540b251 100644 --- a/towr/include/towr/variables/polynomial.h +++ b/towr/include/towr/variables/polynomial.h @@ -51,12 +51,12 @@ namespace towr { * derivatives from the coefficient values. */ class Polynomial { -public: - enum Coefficients { A=0, B, C, D, E, F, G, H, I, J}; + public: + enum Coefficients { A = 0, B, C, D, E, F, G, H, I, J }; using CoeffIDVec = std::vector; using VectorXd = Eigen::VectorXd; -public: + public: /** * @brief Constructs a polynomial with zero coefficient values. * @param poly_order The highest exponent of t, e.g. 5-th order -> t^5. @@ -77,16 +77,16 @@ class Polynomial { * @param poly_deriv Which polynomial derivative f(t), fd(t), function to use. * @param coeff The coefficient with respect to which to calculate the derivative. */ - double GetDerivativeWrtCoeff(double t, Dx poly_deriv, Coefficients coeff) const; + double GetDerivativeWrtCoeff(double t, Dx poly_deriv, + Coefficients coeff) const; -protected: + protected: std::vector coeff_; -private: + private: CoeffIDVec coeff_ids_; }; - /** * @brief Represents a Cubic-Hermite-Polynomial * @@ -107,11 +107,10 @@ class Polynomial { * See also matlab/cubic_hermite_polynomial.m for generation of derivatives. */ class CubicHermitePolynomial : public Polynomial { -public: + public: CubicHermitePolynomial(int dim); virtual ~CubicHermitePolynomial() = default; - /** * @brief sets the total duration of the polynomial. */ @@ -156,9 +155,9 @@ class CubicHermitePolynomial : public Polynomial { */ const double GetDuration() const { return T_; }; -private: - double T_; ///< the total duration of the polynomial. - Node n0_, n1_; ///< the start and final node comprising the polynomial. + private: + double T_; ///< the total duration of the polynomial. + Node n0_, n1_; ///< the start and final node comprising the polynomial. // see matlab/cubic_hermite_polynomial.m script for derivation double GetDerivativeOfPosWrtStartNode(Dx node_deriv, double t_local) const; @@ -170,6 +169,6 @@ class CubicHermitePolynomial : public Polynomial { double GetDerivativeOfAccWrtEndNode(Dx node_deriv, double t_local) const; }; -} // namespace towr +} // namespace towr -#endif // TOWR_VARIABLES_POLYNOMIAL_H_ +#endif // TOWR_VARIABLES_POLYNOMIAL_H_ diff --git a/towr/include/towr/variables/spline.h b/towr/include/towr/variables/spline.h index f90584f9c..41121ae12 100644 --- a/towr/include/towr/variables/spline.h +++ b/towr/include/towr/variables/spline.h @@ -42,13 +42,13 @@ namespace towr { * This class is responsible for stitching together multiple individual * polynomials into one spline. */ -class Spline { -public: +class Spline { + public: using VecTimes = std::vector; using VecPoly = std::vector; Spline(const VecTimes& poly_durations, int n_dim); - virtual ~Spline () = default; + virtual ~Spline() = default; /** * @returns The state of the spline at time t. @@ -85,8 +85,8 @@ class Spline { */ VecTimes GetPolyDurations() const; -protected: - VecPoly cubic_polys_; ///< the sequence of polynomials making up the spline. + protected: + VecPoly cubic_polys_; ///< the sequence of polynomials making up the spline. /** * @brief How much time of the current segment has passed at t_global. @@ -94,7 +94,7 @@ class Spline { * @param d The durations of each segment. * @return The segment id and the time passed in this segment. */ - std::pair GetLocalTime(double t_global, const VecTimes& d) const; + std::pair GetLocalTime(double t_global, const VecTimes& d) const; /** * @brief Updates the cubic-Hermite polynomial coefficients using the diff --git a/towr/include/towr/variables/spline_holder.h b/towr/include/towr/variables/spline_holder.h index ce948564e..7ca81d0b3 100644 --- a/towr/include/towr/variables/spline_holder.h +++ b/towr/include/towr/variables/spline_holder.h @@ -30,10 +30,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_TOWR_INCLUDE_TOWR_VARIABLES_SPLINE_HOLDER_H_ #define TOWR_TOWR_INCLUDE_TOWR_VARIABLES_SPLINE_HOLDER_H_ -#include "phase_durations.h" #include "node_spline.h" #include "nodes_variables.h" #include "nodes_variables_phase_based.h" +#include "phase_durations.h" namespace towr { @@ -55,18 +55,17 @@ struct SplineHolder { * @param phase_durations The phase durations of each endeffector. * @param ee_durations_change True if the ee durations are optimized over. */ - SplineHolder (NodesVariables::Ptr base_lin, - NodesVariables::Ptr base_ang, - const std::vector& base_poly_durations, - std::vector ee_motion, - std::vector ee_force, - std::vector phase_durations, - bool ee_durations_change); + SplineHolder(NodesVariables::Ptr base_lin, NodesVariables::Ptr base_ang, + const std::vector& base_poly_durations, + std::vector ee_motion, + std::vector ee_force, + std::vector phase_durations, + bool ee_durations_change); /** * @brief Attention, nothing initialized. */ - SplineHolder () = default; + SplineHolder() = default; NodeSpline::Ptr base_linear_; NodeSpline::Ptr base_angular_; diff --git a/towr/include/towr/variables/state.h b/towr/include/towr/variables/state.h index 428ad10e0..d305eaf69 100644 --- a/towr/include/towr/variables/state.h +++ b/towr/include/towr/variables/state.h @@ -34,11 +34,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include - namespace towr { ///< the values or derivative. For motions e.g. position, velocity, ... -enum Dx { kPos=0, kVel, kAcc, kJerk }; +enum Dx { kPos = 0, kVel, kAcc, kJerk }; /** * @brief Stores at state comprised of values and higher-order derivatives. @@ -47,7 +46,7 @@ enum Dx { kPos=0, kVel, kAcc, kJerk }; * accelerations, but also a force-profiles with forces, force-derivatives etc. */ class State { -public: + public: using VectorXd = Eigen::VectorXd; /** @@ -89,11 +88,11 @@ class State { */ const VectorXd a() const; -private: - std::vector values_; ///< e.g. position, velocity and acceleration, ... + private: + ///< e.g. position, velocity and acceleration, ... + std::vector values_; }; - /** * @brief A node represents the state of a trajectory at a specific time. * @@ -105,28 +104,26 @@ class State { * acceleration. */ class Node : public State { -public: - static const int n_derivatives = 2; ///< value and first derivative. + public: + static const int n_derivatives = 2; ///< value and first derivative. /** * @brief Constructs a @a dim - dimensional node (default zero-dimensional). */ - explicit Node(int dim = 0) : State(dim, n_derivatives) {}; + explicit Node(int dim = 0) : State(dim, n_derivatives){}; virtual ~Node() = default; }; - /** * @brief Can represent the 6Degree-of-Freedom floating base of a robot. */ struct BaseState { - BaseState(): lin(3), ang(3) {} + BaseState() : lin(3), ang(3) {} - Node lin; ///< linear position x,y,z and velocities. - Node ang; ///< angular euler roll, pitch, yaw and rates. + Node lin; ///< linear position x,y,z and velocities. + Node ang; ///< angular euler roll, pitch, yaw and rates. }; +} // namespace towr -} // namespace towr - -#endif // TOWR_VARIABLES +#endif // TOWR_VARIABLES diff --git a/towr/include/towr/variables/variable_names.h b/towr/include/towr/variables/variable_names.h index cdb5c9240..90542cc38 100644 --- a/towr/include/towr/variables/variable_names.h +++ b/towr/include/towr/variables/variable_names.h @@ -40,31 +40,28 @@ namespace towr { */ namespace id { -static const std::string base_lin_nodes = "base-lin"; -static const std::string base_ang_nodes = "base-ang"; -static const std::string ee_motion_nodes = "ee-motion_"; -static const std::string ee_force_nodes = "ee-force_"; -static const std::string contact_schedule = "ee-schedule"; - +static const std::string base_lin_nodes = "base-lin"; +static const std::string base_ang_nodes = "base-ang"; +static const std::string ee_motion_nodes = "ee-motion_"; +static const std::string ee_force_nodes = "ee-force_"; +static const std::string contact_schedule = "ee-schedule"; static std::string EEMotionNodes(uint ee) { - return ee_motion_nodes + std::to_string(ee); + return ee_motion_nodes + std::to_string(ee); } static std::string EEForceNodes(uint ee) { - return ee_force_nodes + std::to_string(ee); + return ee_force_nodes + std::to_string(ee); } static std::string EESchedule(uint ee) { - return contact_schedule + std::to_string(ee); + return contact_schedule + std::to_string(ee); } -} // namespace id -} // namespace towr - - +} // namespace id +} // namespace towr #endif /* TOWR_VARIABLES_VARIABLE_NAMES_H_ */ diff --git a/towr/src/base_motion_constraint.cc b/towr/src/base_motion_constraint.cc index 818900af1..ea2a664ca 100644 --- a/towr/src/base_motion_constraint.cc +++ b/towr/src/base_motion_constraint.cc @@ -28,16 +28,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ #include -#include #include #include +#include namespace towr { - -BaseMotionConstraint::BaseMotionConstraint (double T, double dt, - const SplineHolder& spline_holder) - :TimeDiscretizationConstraint(T, dt, "baseMotion") +BaseMotionConstraint::BaseMotionConstraint(double T, double dt, + const SplineHolder& spline_holder) + : TimeDiscretizationConstraint(T, dt, "baseMotion") { base_linear_ = spline_holder.base_linear_; base_angular_ = spline_holder.base_angular_; @@ -46,48 +45,48 @@ BaseMotionConstraint::BaseMotionConstraint (double T, double dt, node_bounds_.resize(k6D); node_bounds_.at(AX) = Bounds(-dev_rad, dev_rad); node_bounds_.at(AY) = Bounds(-dev_rad, dev_rad); - node_bounds_.at(AZ) = ifopt::NoBound;//Bounds(-dev_rad, dev_rad); + node_bounds_.at(AZ) = ifopt::NoBound; //Bounds(-dev_rad, dev_rad); - double z_init = base_linear_->GetPoint(0.0).p().z(); + double z_init = base_linear_->GetPoint(0.0).p().z(); node_bounds_.at(LX) = ifopt::NoBound; - node_bounds_.at(LY) = ifopt::NoBound;//Bounds(-0.05, 0.05); - node_bounds_.at(LZ) = Bounds(z_init-0.02, z_init+0.1); // allow to move dev_z cm up and down + node_bounds_.at(LY) = ifopt::NoBound; //Bounds(-0.05, 0.05); + node_bounds_.at(LZ) = Bounds( + z_init - 0.02, z_init + 0.1); // allow to move dev_z cm up and down int n_constraints_per_node = node_bounds_.size(); - SetRows(GetNumberOfNodes()*n_constraints_per_node); + SetRows(GetNumberOfNodes() * n_constraints_per_node); } -void -BaseMotionConstraint::UpdateConstraintAtInstance (double t, int k, - VectorXd& g) const +void BaseMotionConstraint::UpdateConstraintAtInstance(double t, int k, + VectorXd& g) const { g.middleRows(GetRow(k, LX), k3D) = base_linear_->GetPoint(t).p(); g.middleRows(GetRow(k, AX), k3D) = base_angular_->GetPoint(t).p(); } -void -BaseMotionConstraint::UpdateBoundsAtInstance (double t, int k, VecBound& bounds) const +void BaseMotionConstraint::UpdateBoundsAtInstance(double t, int k, + VecBound& bounds) const { - for (int dim=0; dimGetJacobianWrtNodes(t, kPos); + jac.middleRows(GetRow(k, AX), k3D) = + base_angular_->GetJacobianWrtNodes(t, kPos); if (var_set == id::base_lin_nodes) - jac.middleRows(GetRow(k,LX), k3D) = base_linear_->GetJacobianWrtNodes(t, kPos); + jac.middleRows(GetRow(k, LX), k3D) = + base_linear_->GetJacobianWrtNodes(t, kPos); } -int -BaseMotionConstraint::GetRow (int node, int dim) const +int BaseMotionConstraint::GetRow(int node, int dim) const { - return node*node_bounds_.size() + dim; + return node * node_bounds_.size() + dim; } } /* namespace towr */ diff --git a/towr/src/biped_gait_generator.cc b/towr/src/biped_gait_generator.cc index ad580de3a..71fea5dc5 100644 --- a/towr/src/biped_gait_generator.cc +++ b/towr/src/biped_gait_generator.cc @@ -36,191 +36,190 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { -BipedGaitGenerator::BipedGaitGenerator () +BipedGaitGenerator::BipedGaitGenerator() { ContactState init(2, false); I_ = b_ = P_ = B_ = init; P_.at(L) = true; b_.at(R) = true; - B_ = { true, true }; + B_ = {true, true}; SetGaits({Stand}); } -void -BipedGaitGenerator::SetCombo (Combos combo) +void BipedGaitGenerator::SetCombo(Combos combo) { switch (combo) { - case C0: SetGaits({Stand, Walk1, Walk1, Walk1, Walk1, Stand}); break; - case C1: SetGaits({Stand, Run1, Run1, Run1, Run1, Stand}); break; - case C2: SetGaits({Stand, Hop1, Hop1, Hop1, Stand}); break; - case C3: SetGaits({Stand, Hop1, Hop2, Hop2, Stand}); break; - case C4: SetGaits({Stand, Hop5, Hop5, Hop5, Stand}); break; - default: assert(false); std::cout << "Gait not defined\n"; break; + case C0: + SetGaits({Stand, Walk1, Walk1, Walk1, Walk1, Stand}); + break; + case C1: + SetGaits({Stand, Run1, Run1, Run1, Run1, Stand}); + break; + case C2: + SetGaits({Stand, Hop1, Hop1, Hop1, Stand}); + break; + case C3: + SetGaits({Stand, Hop1, Hop2, Hop2, Stand}); + break; + case C4: + SetGaits({Stand, Hop5, Hop5, Hop5, Stand}); + break; + default: + assert(false); + std::cout << "Gait not defined\n"; + break; } } -BipedGaitGenerator::GaitInfo -BipedGaitGenerator::GetGait (Gaits gait) const +BipedGaitGenerator::GaitInfo BipedGaitGenerator::GetGait(Gaits gait) const { switch (gait) { - case Stand: return GetStrideStand(); - case Flight: return GetStrideFlight(); - case Walk1: return GetStrideWalk(); - case Walk2: return GetStrideWalk(); - case Run1: return GetStrideRun(); - case Run3: return GetStrideRun(); - case Hop1: return GetStrideHop(); - case Hop2: return GetStrideLeftHop(); - case Hop3: return GetStrideRightHop(); - case Hop5: return GetStrideGallopHop(); - default: assert(false); // gait not implemented + case Stand: + return GetStrideStand(); + case Flight: + return GetStrideFlight(); + case Walk1: + return GetStrideWalk(); + case Walk2: + return GetStrideWalk(); + case Run1: + return GetStrideRun(); + case Run3: + return GetStrideRun(); + case Hop1: + return GetStrideHop(); + case Hop2: + return GetStrideLeftHop(); + case Hop3: + return GetStrideRightHop(); + case Hop5: + return GetStrideGallopHop(); + default: + assert(false); // gait not implemented } } -BipedGaitGenerator::GaitInfo -BipedGaitGenerator::GetStrideStand () const +BipedGaitGenerator::GaitInfo BipedGaitGenerator::GetStrideStand() const { - auto times = - { + auto times = { 0.2, }; - auto contacts = - { + auto contacts = { B_, }; return std::make_pair(times, contacts); } -BipedGaitGenerator::GaitInfo -BipedGaitGenerator::GetStrideFlight () const +BipedGaitGenerator::GaitInfo BipedGaitGenerator::GetStrideFlight() const { - auto times = - { + auto times = { 0.5, }; - auto contacts = - { + auto contacts = { I_, }; return std::make_pair(times, contacts); } -BipedGaitGenerator::GaitInfo -BipedGaitGenerator::GetStrideWalk () const +BipedGaitGenerator::GaitInfo BipedGaitGenerator::GetStrideWalk() const { - double step = 0.3; + double step = 0.3; double stance = 0.05; - auto times = - { - step, stance, - step, stance, + auto times = { + step, + stance, + step, + stance, }; - auto phase_contacts = - { - b_, B_, // swing left foot - P_, B_, // swing right foot + auto phase_contacts = { + b_, B_, // swing left foot + P_, B_, // swing right foot }; return std::make_pair(times, phase_contacts); } -BipedGaitGenerator::GaitInfo -BipedGaitGenerator::GetStrideRun () const +BipedGaitGenerator::GaitInfo BipedGaitGenerator::GetStrideRun() const { - double flight = 0.4; + double flight = 0.4; double pushoff = 0.15; double landing = 0.15; - auto times = - { - pushoff, flight, - landing+pushoff, flight, landing, + auto times = { + pushoff, flight, landing + pushoff, flight, landing, }; - auto phase_contacts = - { - b_, I_, // swing left foot - P_, I_, b_, // swing right foot + auto phase_contacts = { + b_, I_, // swing left foot + P_, I_, b_, // swing right foot }; return std::make_pair(times, phase_contacts); } -BipedGaitGenerator::GaitInfo -BipedGaitGenerator::GetStrideHop () const +BipedGaitGenerator::GaitInfo BipedGaitGenerator::GetStrideHop() const { - double push = 0.15; - double flight = 0.5; - double land = 0.15; - auto times = - { - push, flight, land - }; - auto phase_contacts = - { - B_, I_, B_, + double push = 0.15; + double flight = 0.5; + double land = 0.15; + auto times = {push, flight, land}; + auto phase_contacts = { + B_, + I_, + B_, }; return std::make_pair(times, phase_contacts); } -BipedGaitGenerator::GaitInfo -BipedGaitGenerator::GetStrideGallopHop () const +BipedGaitGenerator::GaitInfo BipedGaitGenerator::GetStrideGallopHop() const { double push = 0.2; double flight = 0.3; double land = 0.2; - auto times = - { - push, flight, - land, land, + auto times = { + push, + flight, + land, + land, }; - auto phase_contacts = - { - P_, I_, - b_, B_, + auto phase_contacts = { + P_, + I_, + b_, + B_, }; return std::make_pair(times, phase_contacts); } -BipedGaitGenerator::GaitInfo -BipedGaitGenerator::GetStrideLeftHop () const +BipedGaitGenerator::GaitInfo BipedGaitGenerator::GetStrideLeftHop() const { double push = 0.15; double flight = 0.4; double land = 0.15; - auto times = - { - push, flight, land, - }; - auto phase_contacts = - { - b_, I_, b_ + auto times = { + push, + flight, + land, }; + auto phase_contacts = {b_, I_, b_}; return std::make_pair(times, phase_contacts); } -BipedGaitGenerator::GaitInfo -BipedGaitGenerator::GetStrideRightHop () const +BipedGaitGenerator::GaitInfo BipedGaitGenerator::GetStrideRightHop() const { double push = 0.2; double flight = 0.2; double land = 0.2; - auto times = - { - push, flight, land - }; - auto phase_contacts = - { - P_, I_, P_ - }; + auto times = {push, flight, land}; + auto phase_contacts = {P_, I_, P_}; return std::make_pair(times, phase_contacts); } diff --git a/towr/src/dynamic_constraint.cc b/towr/src/dynamic_constraint.cc index f78513d5a..718dd97ff 100644 --- a/towr/src/dynamic_constraint.cc +++ b/towr/src/dynamic_constraint.cc @@ -29,15 +29,15 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include #include +#include namespace towr { -DynamicConstraint::DynamicConstraint (const DynamicModel::Ptr& m, - double T, double dt, - const SplineHolder& spline_holder) - :TimeDiscretizationConstraint(T, dt, "dynamic") +DynamicConstraint::DynamicConstraint(const DynamicModel::Ptr& m, double T, + double dt, + const SplineHolder& spline_holder) + : TimeDiscretizationConstraint(T, dt, "dynamic") { model_ = m; @@ -47,45 +47,44 @@ DynamicConstraint::DynamicConstraint (const DynamicModel::Ptr& m, ee_forces_ = spline_holder.ee_force_; ee_motion_ = spline_holder.ee_motion_; - SetRows(GetNumberOfNodes()*k6D); + SetRows(GetNumberOfNodes() * k6D); } -int -DynamicConstraint::GetRow (int k, Dim6D dimension) const +int DynamicConstraint::GetRow(int k, Dim6D dimension) const { - return k6D*k + dimension; + return k6D * k + dimension; } -void -DynamicConstraint::UpdateConstraintAtInstance(double t, int k, VectorXd& g) const +void DynamicConstraint::UpdateConstraintAtInstance(double t, int k, + VectorXd& g) const { UpdateModel(t); - g.segment(GetRow(k,AX), k6D) = model_->GetDynamicViolation(); + g.segment(GetRow(k, AX), k6D) = model_->GetDynamicViolation(); } -void -DynamicConstraint::UpdateBoundsAtInstance(double t, int k, VecBound& bounds) const +void DynamicConstraint::UpdateBoundsAtInstance(double t, int k, + VecBound& bounds) const { for (auto dim : AllDim6D) - bounds.at(GetRow(k,dim)) = ifopt::BoundZero; + bounds.at(GetRow(k, dim)) = ifopt::BoundZero; } -void -DynamicConstraint::UpdateJacobianAtInstance(double t, int k, std::string var_set, - Jacobian& jac) const +void DynamicConstraint::UpdateJacobianAtInstance(double t, int k, + std::string var_set, + Jacobian& jac) const { UpdateModel(t); int n = jac.cols(); - Jacobian jac_model(k6D,n); + Jacobian jac_model(k6D, n); // sensitivity of dynamic constraint w.r.t base variables. if (var_set == id::base_lin_nodes) { - Jacobian jac_base_lin_pos = base_linear_->GetJacobianWrtNodes(t,kPos); - Jacobian jac_base_lin_acc = base_linear_->GetJacobianWrtNodes(t,kAcc); + Jacobian jac_base_lin_pos = base_linear_->GetJacobianWrtNodes(t, kPos); + Jacobian jac_base_lin_acc = base_linear_->GetJacobianWrtNodes(t, kAcc); - jac_model = model_->GetJacobianWrtBaseLin(jac_base_lin_pos, - jac_base_lin_acc); + jac_model = + model_->GetJacobianWrtBaseLin(jac_base_lin_pos, jac_base_lin_acc); } if (var_set == id::base_ang_nodes) { @@ -93,15 +92,15 @@ DynamicConstraint::UpdateJacobianAtInstance(double t, int k, std::string var_set } // sensitivity of dynamic constraint w.r.t. endeffector variables - for (int ee=0; eeGetEECount(); ++ee) { + for (int ee = 0; ee < model_->GetEECount(); ++ee) { if (var_set == id::EEForceNodes(ee)) { - Jacobian jac_ee_force = ee_forces_.at(ee)->GetJacobianWrtNodes(t,kPos); - jac_model = model_->GetJacobianWrtForce(jac_ee_force, ee); + Jacobian jac_ee_force = ee_forces_.at(ee)->GetJacobianWrtNodes(t, kPos); + jac_model = model_->GetJacobianWrtForce(jac_ee_force, ee); } if (var_set == id::EEMotionNodes(ee)) { - Jacobian jac_ee_pos = ee_motion_.at(ee)->GetJacobianWrtNodes(t,kPos); - jac_model = model_->GetJacobianWrtEEPos(jac_ee_pos, ee); + Jacobian jac_ee_pos = ee_motion_.at(ee)->GetJacobianWrtNodes(t, kPos); + jac_model = model_->GetJacobianWrtEEPos(jac_ee_pos, ee); } if (var_set == id::EESchedule(ee)) { @@ -109,31 +108,31 @@ DynamicConstraint::UpdateJacobianAtInstance(double t, int k, std::string var_set jac_model += model_->GetJacobianWrtForce(jac_f_dT, ee); Jacobian jac_x_dT = ee_motion_.at(ee)->GetJacobianOfPosWrtDurations(t); - jac_model += model_->GetJacobianWrtEEPos(jac_x_dT, ee); + jac_model += model_->GetJacobianWrtEEPos(jac_x_dT, ee); } } - jac.middleRows(GetRow(k,AX), k6D) = jac_model; + jac.middleRows(GetRow(k, AX), k6D) = jac_model; } -void -DynamicConstraint::UpdateModel (double t) const +void DynamicConstraint::UpdateModel(double t) const { auto com = base_linear_->GetPoint(t); - Eigen::Matrix3d w_R_b = base_angular_.GetRotationMatrixBaseToWorld(t); - Eigen::Vector3d omega = base_angular_.GetAngularVelocityInWorld(t); + Eigen::Matrix3d w_R_b = base_angular_.GetRotationMatrixBaseToWorld(t); + Eigen::Vector3d omega = base_angular_.GetAngularVelocityInWorld(t); Eigen::Vector3d omega_dot = base_angular_.GetAngularAccelerationInWorld(t); int n_ee = model_->GetEECount(); std::vector ee_pos; std::vector ee_force; - for (int ee=0; eeGetPoint(t).p()); ee_pos.push_back(ee_motion_.at(ee)->GetPoint(t).p()); } - model_->SetCurrent(com.p(), com.a(), w_R_b, omega, omega_dot, ee_force, ee_pos); + model_->SetCurrent(com.p(), com.a(), w_R_b, omega, omega_dot, ee_force, + ee_pos); } } /* namespace towr */ diff --git a/towr/src/dynamic_model.cc b/towr/src/dynamic_model.cc index 94255655d..c315f74eb 100644 --- a/towr/src/dynamic_model.cc +++ b/towr/src/dynamic_model.cc @@ -41,26 +41,26 @@ DynamicModel::DynamicModel(double mass, int ee_count) w_R_b_.setIdentity(); omega_.setZero(); - omega_dot_ .setZero(); + omega_dot_.setZero(); ee_force_ = EELoad(ee_count); - ee_pos_ = EEPos(ee_count); + ee_pos_ = EEPos(ee_count); } -void -DynamicModel::SetCurrent (const ComPos& com_W, const Vector3d com_acc_W, - const Matrix3d& w_R_b, const AngVel& omega_W, const Vector3d& omega_dot_W, - const EELoad& force_W, const EEPos& pos_W) +void DynamicModel::SetCurrent(const ComPos& com_W, const Vector3d com_acc_W, + const Matrix3d& w_R_b, const AngVel& omega_W, + const Vector3d& omega_dot_W, + const EELoad& force_W, const EEPos& pos_W) { - com_pos_ = com_W; - com_acc_ = com_acc_W; + com_pos_ = com_W; + com_acc_ = com_acc_W; w_R_b_ = w_R_b; omega_ = omega_W; omega_dot_ = omega_dot_W; - ee_force_ = force_W; - ee_pos_ = pos_W; + ee_force_ = force_W; + ee_pos_ = pos_W; } } /* namespace towr */ diff --git a/towr/src/euler_converter.cc b/towr/src/euler_converter.cc index ec0385e3b..754237d7e 100644 --- a/towr/src/euler_converter.cc +++ b/towr/src/euler_converter.cc @@ -34,104 +34,94 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { - -EulerConverter::EulerConverter (const NodeSpline::Ptr& euler) +EulerConverter::EulerConverter(const NodeSpline::Ptr& euler) { - euler_ = euler; + euler_ = euler; jac_wrt_nodes_structure_ = Jacobian(k3D, euler->GetNodeVariablesCount()); } -Eigen::Quaterniond -EulerConverter::GetQuaternionBaseToWorld (double t) const +Eigen::Quaterniond EulerConverter::GetQuaternionBaseToWorld(double t) const { State ori = euler_->GetPoint(t); return GetQuaternionBaseToWorld(ori.p()); } -Eigen::Quaterniond -EulerConverter::GetQuaternionBaseToWorld (const EulerAngles& pos) +Eigen::Quaterniond EulerConverter::GetQuaternionBaseToWorld( + const EulerAngles& pos) { Eigen::Matrix3d R_WB = GetRotationMatrixBaseToWorld(pos); return Eigen::Quaterniond(R_WB); } -Eigen::Vector3d -EulerConverter::GetAngularVelocityInWorld (double t) const +Eigen::Vector3d EulerConverter::GetAngularVelocityInWorld(double t) const { State ori = euler_->GetPoint(t); return GetAngularVelocityInWorld(ori.p(), ori.v()); } -Eigen::Vector3d -EulerConverter::GetAngularVelocityInWorld (const EulerAngles& pos, - const EulerRates& vel) +Eigen::Vector3d EulerConverter::GetAngularVelocityInWorld( + const EulerAngles& pos, const EulerRates& vel) { - return GetM(pos)*vel; + return GetM(pos) * vel; } -Eigen::Vector3d -EulerConverter::GetAngularAccelerationInWorld (double t) const +Eigen::Vector3d EulerConverter::GetAngularAccelerationInWorld(double t) const { State ori = euler_->GetPoint(t); return GetAngularAccelerationInWorld(ori); } -Eigen::Vector3d -EulerConverter::GetAngularAccelerationInWorld (State ori) +Eigen::Vector3d EulerConverter::GetAngularAccelerationInWorld(State ori) { - return GetMdot(ori.p(), ori.v())*ori.v() + GetM(ori.p())*ori.a(); + return GetMdot(ori.p(), ori.v()) * ori.v() + GetM(ori.p()) * ori.a(); } -EulerConverter::Jacobian -EulerConverter::GetDerivOfAngVelWrtEulerNodes(double t) const +EulerConverter::Jacobian EulerConverter::GetDerivOfAngVelWrtEulerNodes( + double t) const { Jacobian jac = jac_wrt_nodes_structure_; State ori = euler_->GetPoint(t); // convert to sparse, but also regard 0.0 as non-zero element, because // could turn nonzero during the course of the program - JacobianRow vel = ori.v().transpose().sparseView(1.0, -1.0); - Jacobian dVel_du = euler_->GetJacobianWrtNodes(t, kVel); + JacobianRow vel = ori.v().transpose().sparseView(1.0, -1.0); + Jacobian dVel_du = euler_->GetJacobianWrtNodes(t, kVel); - for (auto dim : {X,Y,Z}) { - Jacobian dM_du = GetDerivMwrtNodes(t,dim); - jac.row(dim) = vel*dM_du + GetM(ori.p()).row(dim)*dVel_du; + for (auto dim : {X, Y, Z}) { + Jacobian dM_du = GetDerivMwrtNodes(t, dim); + jac.row(dim) = vel * dM_du + GetM(ori.p()).row(dim) * dVel_du; } return jac; } -EulerConverter::Jacobian -EulerConverter::GetDerivOfAngAccWrtEulerNodes (double t) const +EulerConverter::Jacobian EulerConverter::GetDerivOfAngAccWrtEulerNodes( + double t) const { Jacobian jac = jac_wrt_nodes_structure_; - State ori = euler_->GetPoint(t); // convert to sparse, but also regard 0.0 as non-zero element, because // could turn nonzero during the course of the program JacobianRow vel = ori.v().transpose().sparseView(1.0, -1.0); JacobianRow acc = ori.a().transpose().sparseView(1.0, -1.0); - Jacobian dVel_du = euler_->GetJacobianWrtNodes(t, kVel); - Jacobian dAcc_du = euler_->GetJacobianWrtNodes(t, kAcc); + Jacobian dVel_du = euler_->GetJacobianWrtNodes(t, kVel); + Jacobian dAcc_du = euler_->GetJacobianWrtNodes(t, kAcc); + for (auto dim : {X, Y, Z}) { + Jacobian dMdot_du = GetDerivMdotwrtNodes(t, dim); + Jacobian dM_du = GetDerivMwrtNodes(t, dim); - for (auto dim : {X,Y,Z}) { - Jacobian dMdot_du = GetDerivMdotwrtNodes(t,dim); - Jacobian dM_du = GetDerivMwrtNodes(t,dim); - - jac.row(dim) = vel * dMdot_du - + GetMdot(ori.p(), ori.v()).row(dim)* dVel_du - + acc * dM_du - + GetM(ori.p()).row(dim) * dAcc_du; + jac.row(dim) = vel * dMdot_du + + GetMdot(ori.p(), ori.v()).row(dim) * dVel_du + acc * dM_du + + GetM(ori.p()).row(dim) * dAcc_du; } return jac; } -EulerConverter::MatrixSXd -EulerConverter::GetM (const EulerAngles& xyz) +EulerConverter::MatrixSXd EulerConverter::GetM(const EulerAngles& xyz) { double z = xyz(Z); double y = xyz(Y); @@ -140,16 +130,18 @@ EulerConverter::GetM (const EulerAngles& xyz) // http://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf Jacobian M(k3D, k3D); - /* - */ M.coeffRef(0,Y) = -sin(z); M.coeffRef(0,X) = cos(y)*cos(z); - /* - */ M.coeffRef(1,Y) = cos(z); M.coeffRef(1,X) = cos(y)*sin(z); - M.coeffRef(2,Z) = 1.0; /* - */ M.coeffRef(2,X) = -sin(y); + /* - */ M.coeffRef(0, Y) = -sin(z); + M.coeffRef(0, X) = cos(y) * cos(z); + /* - */ M.coeffRef(1, Y) = cos(z); + M.coeffRef(1, X) = cos(y) * sin(z); + M.coeffRef(2, Z) = 1.0; /* - */ + M.coeffRef(2, X) = -sin(y); return M; } -EulerConverter::MatrixSXd -EulerConverter::GetMdot (const EulerAngles& xyz, - const EulerRates& xyz_d) +EulerConverter::MatrixSXd EulerConverter::GetMdot(const EulerAngles& xyz, + const EulerRates& xyz_d) { double z = xyz(Z); double zd = xyz_d(Z); @@ -158,36 +150,38 @@ EulerConverter::GetMdot (const EulerAngles& xyz, Jacobian Mdot(k3D, k3D); - Mdot.coeffRef(0,Y) = -cos(z)*zd; Mdot.coeffRef(0,X) = -cos(z)*sin(y)*yd - cos(y)*sin(z)*zd; - Mdot.coeffRef(1,Y) = -sin(z)*zd; Mdot.coeffRef(1,X) = cos(y)*cos(z)*zd - sin(y)*sin(z)*yd; - /* - */ Mdot.coeffRef(2,X) = -cos(y)*yd; + Mdot.coeffRef(0, Y) = -cos(z) * zd; + Mdot.coeffRef(0, X) = -cos(z) * sin(y) * yd - cos(y) * sin(z) * zd; + Mdot.coeffRef(1, Y) = -sin(z) * zd; + Mdot.coeffRef(1, X) = cos(y) * cos(z) * zd - sin(y) * sin(z) * yd; + /* - */ Mdot.coeffRef(2, X) = -cos(y) * yd; - return Mdot; + return Mdot; } -EulerConverter::Jacobian -EulerConverter::GetDerivMwrtNodes (double t, Dim3D ang_acc_dim) const +EulerConverter::Jacobian EulerConverter::GetDerivMwrtNodes( + double t, Dim3D ang_acc_dim) const { State ori = euler_->GetPoint(t); - double z = ori.p()(Z); - double y = ori.p()(Y); + double z = ori.p()(Z); + double y = ori.p()(Y); JacobianRow jac_z = GetJac(t, kPos, Z); JacobianRow jac_y = GetJac(t, kPos, Y); Jacobian jac = jac_wrt_nodes_structure_; switch (ang_acc_dim) { - case X: // basically derivative of top row (3 elements) of matrix M - jac.row(Y) = -cos(z)*jac_z; - jac.row(X) = -cos(z)*sin(y)*jac_y - cos(y)*sin(z)*jac_z; + case X: // basically derivative of top row (3 elements) of matrix M + jac.row(Y) = -cos(z) * jac_z; + jac.row(X) = -cos(z) * sin(y) * jac_y - cos(y) * sin(z) * jac_z; break; - case Y: // middle row of M - jac.row(Y) = -sin(z)*jac_z; - jac.row(X) = cos(y)*cos(z)*jac_z - sin(y)*sin(z)*jac_y; + case Y: // middle row of M + jac.row(Y) = -sin(z) * jac_z; + jac.row(X) = cos(y) * cos(z) * jac_z - sin(y) * sin(z) * jac_y; break; - case Z: // bottom row of M - jac.row(X) = -cos(y)*jac_y; + case Z: // bottom row of M + jac.row(X) = -cos(y) * jac_y; break; default: assert(false); @@ -197,15 +191,15 @@ EulerConverter::GetDerivMwrtNodes (double t, Dim3D ang_acc_dim) const return jac; } -EulerConverter::MatrixSXd -EulerConverter::GetRotationMatrixBaseToWorld (double t) const +EulerConverter::MatrixSXd EulerConverter::GetRotationMatrixBaseToWorld( + double t) const { State ori = euler_->GetPoint(t); return GetRotationMatrixBaseToWorld(ori.p()); } -EulerConverter::MatrixSXd -EulerConverter::GetRotationMatrixBaseToWorld (const EulerAngles& xyz) +EulerConverter::MatrixSXd EulerConverter::GetRotationMatrixBaseToWorld( + const EulerAngles& xyz) { double x = xyz(X); double y = xyz(Y); @@ -213,25 +207,28 @@ EulerConverter::GetRotationMatrixBaseToWorld (const EulerAngles& xyz) Eigen::Matrix3d M; // http://docs.leggedrobotics.com/kindr/cheatsheet_latest.pdf (Euler ZYX) - M << cos(y)*cos(z), cos(z)*sin(x)*sin(y) - cos(x)*sin(z), sin(x)*sin(z) + cos(x)*cos(z)*sin(y), - cos(y)*sin(z), cos(x)*cos(z) + sin(x)*sin(y)*sin(z), cos(x)*sin(y)*sin(z) - cos(z)*sin(x), - -sin(y), cos(y)*sin(x), cos(x)*cos(y); + M << cos(y) * cos(z), cos(z) * sin(x) * sin(y) - cos(x) * sin(z), + sin(x) * sin(z) + cos(x) * cos(z) * sin(y), cos(y) * sin(z), + cos(x) * cos(z) + sin(x) * sin(y) * sin(z), + cos(x) * sin(y) * sin(z) - cos(z) * sin(x), -sin(y), cos(y) * sin(x), + cos(x) * cos(y); return M.sparseView(1.0, -1.0); } -EulerConverter::Jacobian -EulerConverter::DerivOfRotVecMult (double t, const Vector3d& v, bool inverse) const +EulerConverter::Jacobian EulerConverter::DerivOfRotVecMult(double t, + const Vector3d& v, + bool inverse) const { JacRowMatrix Rd = GetDerivativeOfRotationMatrixWrtNodes(t); - Jacobian jac = jac_wrt_nodes_structure_; + Jacobian jac = jac_wrt_nodes_structure_; - for (int row : {X,Y,Z}) { + for (int row : {X, Y, Z}) { for (int col : {X, Y, Z}) { // since for every rotation matrix R^(-1) = R^T, just swap rows and // columns for calculation of derivative of inverse rotation matrix - JacobianRow jac_row = inverse? Rd.at(col).at(row) : Rd.at(row).at(col); - jac.row(row) += v(col)*jac_row; + JacobianRow jac_row = inverse ? Rd.at(col).at(row) : Rd.at(row).at(col); + jac.row(row) += v(col) * jac_row; } } @@ -239,36 +236,47 @@ EulerConverter::DerivOfRotVecMult (double t, const Vector3d& v, bool inverse) co } EulerConverter::JacRowMatrix -EulerConverter::GetDerivativeOfRotationMatrixWrtNodes (double t) const +EulerConverter::GetDerivativeOfRotationMatrixWrtNodes(double t) const { JacRowMatrix jac; State ori = euler_->GetPoint(t); - double x = ori.p()(X); - double y = ori.p()(Y); - double z = ori.p()(Z); + double x = ori.p()(X); + double y = ori.p()(Y); + double z = ori.p()(Z); JacobianRow jac_x = GetJac(t, kPos, X); JacobianRow jac_y = GetJac(t, kPos, Y); JacobianRow jac_z = GetJac(t, kPos, Z); - jac.at(X).at(X) = -cos(z)*sin(y)*jac_y - cos(y)*sin(z)*jac_z; - jac.at(X).at(Y) = sin(x)*sin(z)*jac_x - cos(x)*cos(z)*jac_z - sin(x)*sin(y)*sin(z)*jac_z + cos(x)*cos(z)*sin(y)*jac_x + cos(y)*cos(z)*sin(x)*jac_y; - jac.at(X).at(Z) = cos(x)*sin(z)*jac_x + cos(z)*sin(x)*jac_z - cos(z)*sin(x)*sin(y)*jac_x - cos(x)*sin(y)*sin(z)*jac_z + cos(x)*cos(y)*cos(z)*jac_y; - - jac.at(Y).at(X) = cos(y)*cos(z)*jac_z - sin(y)*sin(z)*jac_y; - jac.at(Y).at(Y) = cos(x)*sin(y)*sin(z)*jac_x - cos(x)*sin(z)*jac_z - cos(z)*sin(x)*jac_x + cos(y)*sin(x)*sin(z)*jac_y + cos(z)*sin(x)*sin(y)*jac_z; - jac.at(Y).at(Z) = sin(x)*sin(z)*jac_z - cos(x)*cos(z)*jac_x - sin(x)*sin(y)*sin(z)*jac_x + cos(x)*cos(y)*sin(z)*jac_y + cos(x)*cos(z)*sin(y)*jac_z; - - jac.at(Z).at(X) = -cos(y)*jac_y; - jac.at(Z).at(Y) = cos(x)*cos(y)*jac_x - sin(x)*sin(y)*jac_y; - jac.at(Z).at(Z) = -cos(y)*sin(x)*jac_x - cos(x)*sin(y)*jac_y; + jac.at(X).at(X) = -cos(z) * sin(y) * jac_y - cos(y) * sin(z) * jac_z; + jac.at(X).at(Y) = sin(x) * sin(z) * jac_x - cos(x) * cos(z) * jac_z - + sin(x) * sin(y) * sin(z) * jac_z + + cos(x) * cos(z) * sin(y) * jac_x + + cos(y) * cos(z) * sin(x) * jac_y; + jac.at(X).at(Z) = cos(x) * sin(z) * jac_x + cos(z) * sin(x) * jac_z - + cos(z) * sin(x) * sin(y) * jac_x - + cos(x) * sin(y) * sin(z) * jac_z + + cos(x) * cos(y) * cos(z) * jac_y; + + jac.at(Y).at(X) = cos(y) * cos(z) * jac_z - sin(y) * sin(z) * jac_y; + jac.at(Y).at(Y) = cos(x) * sin(y) * sin(z) * jac_x - cos(x) * sin(z) * jac_z - + cos(z) * sin(x) * jac_x + cos(y) * sin(x) * sin(z) * jac_y + + cos(z) * sin(x) * sin(y) * jac_z; + jac.at(Y).at(Z) = sin(x) * sin(z) * jac_z - cos(x) * cos(z) * jac_x - + sin(x) * sin(y) * sin(z) * jac_x + + cos(x) * cos(y) * sin(z) * jac_y + + cos(x) * cos(z) * sin(y) * jac_z; + + jac.at(Z).at(X) = -cos(y) * jac_y; + jac.at(Z).at(Y) = cos(x) * cos(y) * jac_x - sin(x) * sin(y) * jac_y; + jac.at(Z).at(Z) = -cos(y) * sin(x) * jac_x - cos(x) * sin(y) * jac_y; return jac; } -EulerConverter::Jacobian -EulerConverter::GetDerivMdotwrtNodes (double t, Dim3D ang_acc_dim) const +EulerConverter::Jacobian EulerConverter::GetDerivMdotwrtNodes( + double t, Dim3D ang_acc_dim) const { State ori = euler_->GetPoint(t); @@ -284,16 +292,20 @@ EulerConverter::GetDerivMdotwrtNodes (double t, Dim3D ang_acc_dim) const Jacobian jac = jac_wrt_nodes_structure_; switch (ang_acc_dim) { - case X: // derivative of top row (3 elements) of matrix M-dot - jac.row(Y) = sin(z)*zd*jac_z - cos(z)*jac_zd; - jac.row(X) = sin(y)*sin(z)*yd*jac_z - cos(y)*sin(z)*jac_zd - cos(y)*cos(z)*yd*jac_y - cos(y)*cos(z)*zd*jac_z - cos(z)*sin(y)*jac_yd + sin(y)*sin(z)*jac_y*zd; + case X: // derivative of top row (3 elements) of matrix M-dot + jac.row(Y) = sin(z) * zd * jac_z - cos(z) * jac_zd; + jac.row(X) = sin(y) * sin(z) * yd * jac_z - cos(y) * sin(z) * jac_zd - + cos(y) * cos(z) * yd * jac_y - cos(y) * cos(z) * zd * jac_z - + cos(z) * sin(y) * jac_yd + sin(y) * sin(z) * jac_y * zd; break; - case Y: // middle row of M - jac.row(Y) = - sin(z)*jac_zd - cos(z)*zd*jac_z; - jac.row(X) = cos(y)*cos(z)*jac_zd - sin(y)*sin(z)*jac_yd - cos(y)*sin(z)*yd*jac_y - cos(z)*sin(y)*yd*jac_z - cos(z)*sin(y)*jac_y*zd - cos(y)*sin(z)*zd*jac_z; + case Y: // middle row of M + jac.row(Y) = -sin(z) * jac_zd - cos(z) * zd * jac_z; + jac.row(X) = cos(y) * cos(z) * jac_zd - sin(y) * sin(z) * jac_yd - + cos(y) * sin(z) * yd * jac_y - cos(z) * sin(y) * yd * jac_z - + cos(z) * sin(y) * jac_y * zd - cos(y) * sin(z) * zd * jac_z; break; - case Z: // bottom Row of M - jac.row(X) = sin(y)*yd*jac_y - cos(y)*jac_yd; + case Z: // bottom Row of M + jac.row(X) = sin(y) * yd * jac_y - cos(y) * jac_yd; break; default: assert(false); @@ -303,8 +315,8 @@ EulerConverter::GetDerivMdotwrtNodes (double t, Dim3D ang_acc_dim) const return jac; } -EulerConverter::JacobianRow -EulerConverter::GetJac (double t, Dx deriv, Dim3D dim) const +EulerConverter::JacobianRow EulerConverter::GetJac(double t, Dx deriv, + Dim3D dim) const { return euler_->GetJacobianWrtNodes(t, deriv).row(dim); } diff --git a/towr/src/force_constraint.cc b/towr/src/force_constraint.cc index f27fb7e26..ee67339af 100644 --- a/towr/src/force_constraint.cc +++ b/towr/src/force_constraint.cc @@ -33,136 +33,149 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { - -ForceConstraint::ForceConstraint (const HeightMap::Ptr& terrain, - double force_limit, - EE ee) - :ifopt::ConstraintSet(kSpecifyLater, "force-" + id::EEForceNodes(ee)) +ForceConstraint::ForceConstraint(const HeightMap::Ptr& terrain, + double force_limit, EE ee) + : ifopt::ConstraintSet(kSpecifyLater, "force-" + id::EEForceNodes(ee)) { terrain_ = terrain; fn_max_ = force_limit; mu_ = terrain->GetFrictionCoeff(); ee_ = ee; - n_constraints_per_node_ = 1 + 2*k2D; // positive normal force + 4 friction pyramid constraints + // positive normal force + 4 friction pyramid constraints + n_constraints_per_node_ = 1 + 2 * k2D; } -void -ForceConstraint::InitVariableDependedQuantities (const VariablesPtr& x) +void ForceConstraint::InitVariableDependedQuantities(const VariablesPtr& x) { - ee_force_ = x->GetComponent(id::EEForceNodes(ee_)); - ee_motion_ = x->GetComponent(id::EEMotionNodes(ee_)); + ee_force_ = x->GetComponent(id::EEForceNodes(ee_)); + ee_motion_ = + x->GetComponent(id::EEMotionNodes(ee_)); pure_stance_force_node_ids_ = ee_force_->GetIndicesOfNonConstantNodes(); - int constraint_count = pure_stance_force_node_ids_.size()*n_constraints_per_node_; + int constraint_count = + pure_stance_force_node_ids_.size() * n_constraints_per_node_; SetRows(constraint_count); } -Eigen::VectorXd -ForceConstraint::GetValues () const +Eigen::VectorXd ForceConstraint::GetValues() const { VectorXd g(GetRows()); - int row=0; + int row = 0; auto force_nodes = ee_force_->GetNodes(); for (int f_node_id : pure_stance_force_node_ids_) { int phase = ee_force_->GetPhase(f_node_id); - Vector3d p = ee_motion_->GetValueAtStartOfPhase(phase); // doesn't change during stance phase + Vector3d p = ee_motion_->GetValueAtStartOfPhase( + phase); // doesn't change during stance phase Vector3d n = terrain_->GetNormalizedBasis(HeightMap::Normal, p.x(), p.y()); Vector3d f = force_nodes.at(f_node_id).p(); // unilateral force - g(row++) = f.transpose() * n; // >0 (unilateral forces) + g(row++) = f.transpose() * n; // >0 (unilateral forces) // frictional pyramid - Vector3d t1 = terrain_->GetNormalizedBasis(HeightMap::Tangent1, p.x(), p.y()); - g(row++) = f.transpose() * (t1 - mu_*n); // t1 < mu*n - g(row++) = f.transpose() * (t1 + mu_*n); // t1 > -mu*n - - Vector3d t2 = terrain_->GetNormalizedBasis(HeightMap::Tangent2, p.x(), p.y()); - g(row++) = f.transpose() * (t2 - mu_*n); // t2 < mu*n - g(row++) = f.transpose() * (t2 + mu_*n); // t2 > -mu*n + Vector3d t1 = + terrain_->GetNormalizedBasis(HeightMap::Tangent1, p.x(), p.y()); + g(row++) = f.transpose() * (t1 - mu_ * n); // t1 < mu*n + g(row++) = f.transpose() * (t1 + mu_ * n); // t1 > -mu*n + + Vector3d t2 = + terrain_->GetNormalizedBasis(HeightMap::Tangent2, p.x(), p.y()); + g(row++) = f.transpose() * (t2 - mu_ * n); // t2 < mu*n + g(row++) = f.transpose() * (t2 + mu_ * n); // t2 > -mu*n } return g; } -ForceConstraint::VecBound -ForceConstraint::GetBounds () const +ForceConstraint::VecBound ForceConstraint::GetBounds() const { VecBound bounds; for (int f_node_id : pure_stance_force_node_ids_) { - bounds.push_back(ifopt::Bounds(0.0, fn_max_)); // unilateral forces - bounds.push_back(ifopt::BoundSmallerZero); // f_t1 < mu*n - bounds.push_back(ifopt::BoundGreaterZero); // f_t1 > -mu*n - bounds.push_back(ifopt::BoundSmallerZero); // f_t2 < mu*n - bounds.push_back(ifopt::BoundGreaterZero); // f_t2 > -mu*n + bounds.push_back(ifopt::Bounds(0.0, fn_max_)); // unilateral forces + bounds.push_back(ifopt::BoundSmallerZero); // f_t1 < mu*n + bounds.push_back(ifopt::BoundGreaterZero); // f_t1 > -mu*n + bounds.push_back(ifopt::BoundSmallerZero); // f_t2 < mu*n + bounds.push_back(ifopt::BoundGreaterZero); // f_t2 > -mu*n } return bounds; } -void -ForceConstraint::FillJacobianBlock (std::string var_set, - Jacobian& jac) const +void ForceConstraint::FillJacobianBlock(std::string var_set, + Jacobian& jac) const { if (var_set == ee_force_->GetName()) { int row = 0; for (int f_node_id : pure_stance_force_node_ids_) { // unilateral force - int phase = ee_force_->GetPhase(f_node_id); - Vector3d p = ee_motion_->GetValueAtStartOfPhase(phase); // doesn't change during phase - Vector3d n = terrain_->GetNormalizedBasis(HeightMap::Normal, p.x(), p.y()); - Vector3d t1 = terrain_->GetNormalizedBasis(HeightMap::Tangent1, p.x(), p.y()); - Vector3d t2 = terrain_->GetNormalizedBasis(HeightMap::Tangent2, p.x(), p.y()); - - for (auto dim : {X,Y,Z}) { - int idx = ee_force_->GetOptIndex(NodesVariables::NodeValueInfo(f_node_id, kPos, dim)); - - int row_reset=row; - - jac.coeffRef(row_reset++, idx) = n(dim); // unilateral force - jac.coeffRef(row_reset++, idx) = t1(dim)-mu_*n(dim); // f_t1 < mu*n - jac.coeffRef(row_reset++, idx) = t1(dim)+mu_*n(dim); // f_t1 > -mu*n - jac.coeffRef(row_reset++, idx) = t2(dim)-mu_*n(dim); // f_t2 < mu*n - jac.coeffRef(row_reset++, idx) = t2(dim)+mu_*n(dim); // f_t2 > -mu*n + int phase = ee_force_->GetPhase(f_node_id); + Vector3d p = ee_motion_->GetValueAtStartOfPhase( + phase); // doesn't change during phase + Vector3d n = + terrain_->GetNormalizedBasis(HeightMap::Normal, p.x(), p.y()); + Vector3d t1 = + terrain_->GetNormalizedBasis(HeightMap::Tangent1, p.x(), p.y()); + Vector3d t2 = + terrain_->GetNormalizedBasis(HeightMap::Tangent2, p.x(), p.y()); + + for (auto dim : {X, Y, Z}) { + int idx = ee_force_->GetOptIndex( + NodesVariables::NodeValueInfo(f_node_id, kPos, dim)); + + int row_reset = row; + + jac.coeffRef(row_reset++, idx) = n(dim); // unilateral force + // f_t1 < mu*n + jac.coeffRef(row_reset++, idx) = t1(dim) - mu_ * n(dim); + // f_t1 > -mu*n + jac.coeffRef(row_reset++, idx) = t1(dim) + mu_ * n(dim); + // f_t2 < mu*n + jac.coeffRef(row_reset++, idx) = t2(dim) - mu_ * n(dim); + // f_t2 > -mu*n + jac.coeffRef(row_reset++, idx) = t2(dim) + mu_ * n(dim); } row += n_constraints_per_node_; } } - if (var_set == ee_motion_->GetName()) { - int row = 0; + int row = 0; auto force_nodes = ee_force_->GetNodes(); for (int f_node_id : pure_stance_force_node_ids_) { - int phase = ee_force_->GetPhase(f_node_id); + int phase = ee_force_->GetPhase(f_node_id); int ee_node_id = ee_motion_->GetNodeIDAtStartOfPhase(phase); - Vector3d p = ee_motion_->GetValueAtStartOfPhase(phase); // doesn't change during pahse + // doesn't change during pahse + Vector3d p = ee_motion_->GetValueAtStartOfPhase(phase); Vector3d f = force_nodes.at(f_node_id).p(); - for (auto dim : {X_,Y_}) { - Vector3d dn = terrain_->GetDerivativeOfNormalizedBasisWrt(HeightMap::Normal, dim, p.x(), p.y()); - Vector3d dt1 = terrain_->GetDerivativeOfNormalizedBasisWrt(HeightMap::Tangent1, dim, p.x(), p.y()); - Vector3d dt2 = terrain_->GetDerivativeOfNormalizedBasisWrt(HeightMap::Tangent2, dim, p.x(), p.y()); + for (auto dim : {X_, Y_}) { + Vector3d dn = terrain_->GetDerivativeOfNormalizedBasisWrt( + HeightMap::Normal, dim, p.x(), p.y()); + Vector3d dt1 = terrain_->GetDerivativeOfNormalizedBasisWrt( + HeightMap::Tangent1, dim, p.x(), p.y()); + Vector3d dt2 = terrain_->GetDerivativeOfNormalizedBasisWrt( + HeightMap::Tangent2, dim, p.x(), p.y()); - int idx = ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(ee_node_id, kPos, dim)); - int row_reset=row; + int idx = ee_motion_->GetOptIndex( + NodesVariables::NodeValueInfo(ee_node_id, kPos, dim)); + int row_reset = row; // unilateral force - jac.coeffRef(row_reset++, idx) = f.transpose()*dn; + jac.coeffRef(row_reset++, idx) = f.transpose() * dn; // friction force tangent 1 derivative - jac.coeffRef(row_reset++, idx) = f.transpose()*(dt1-mu_*dn); - jac.coeffRef(row_reset++, idx) = f.transpose()*(dt1+mu_*dn); + jac.coeffRef(row_reset++, idx) = f.transpose() * (dt1 - mu_ * dn); + jac.coeffRef(row_reset++, idx) = f.transpose() * (dt1 + mu_ * dn); // friction force tangent 2 derivative - jac.coeffRef(row_reset++, idx) = f.transpose()*(dt2-mu_*dn); - jac.coeffRef(row_reset++, idx) = f.transpose()*(dt2+mu_*dn); + jac.coeffRef(row_reset++, idx) = f.transpose() * (dt2 - mu_ * dn); + jac.coeffRef(row_reset++, idx) = f.transpose() * (dt2 + mu_ * dn); } row += n_constraints_per_node_; diff --git a/towr/src/gait_generator.cc b/towr/src/gait_generator.cc index 7dfdd1ff9..16a6d044f 100644 --- a/towr/src/gait_generator.cc +++ b/towr/src/gait_generator.cc @@ -29,67 +29,71 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include +#include // std::transform #include -#include // std::accumulate -#include // std::transform +#include // std::accumulate -#include #include +#include #include namespace towr { - -GaitGenerator::Ptr -GaitGenerator::MakeGaitGenerator(int leg_count) +GaitGenerator::Ptr GaitGenerator::MakeGaitGenerator(int leg_count) { switch (leg_count) { - case 1: return std::make_shared(); break; - case 2: return std::make_shared(); break; - case 4: return std::make_shared(); break; - default: assert(false); break; // Error: Not implemented + case 1: + return std::make_shared(); + break; + case 2: + return std::make_shared(); + break; + case 4: + return std::make_shared(); + break; + default: + assert(false); + break; // Error: Not implemented } } -GaitGenerator::VecTimes -GaitGenerator::GetPhaseDurations (double t_total, EE ee) const +GaitGenerator::VecTimes GaitGenerator::GetPhaseDurations(double t_total, + EE ee) const { // scale total time tu t_total std::vector durations; for (auto d : GetNormalizedPhaseDurations(ee)) - durations.push_back(d*t_total); + durations.push_back(d * t_total); return durations; } -GaitGenerator::VecTimes -GaitGenerator::GetNormalizedPhaseDurations (EE ee) const +GaitGenerator::VecTimes GaitGenerator::GetNormalizedPhaseDurations(EE ee) const { - auto v = GetPhaseDurations().at(ee); // shorthand + auto v = GetPhaseDurations().at(ee); // shorthand double total_time = std::accumulate(v.begin(), v.end(), 0.0); std::transform(v.begin(), v.end(), v.begin(), - [total_time](double t_phase){ return t_phase/total_time;}); + [total_time](double t_phase) { return t_phase / total_time; }); return v; } -GaitGenerator::FootDurations -GaitGenerator::GetPhaseDurations () const +GaitGenerator::FootDurations GaitGenerator::GetPhaseDurations() const { int n_ee = contacts_.front().size(); VecTimes d_accumulated(n_ee, 0.0); FootDurations foot_durations(n_ee); - for (int phase=0; phase& gaits) +void GaitGenerator::SetGaits(const std::vector& gaits) { contacts_.clear(); times_.clear(); @@ -119,17 +120,16 @@ GaitGenerator::SetGaits (const std::vector& gaits) for (Gaits g : gaits) { auto info = GetGait(g); - std::vector t = info.first; + std::vector t = info.first; std::vector c = info.second; - assert(t.size() == c.size()); // make sure every phase has a time + assert(t.size() == c.size()); // make sure every phase has a time - times_.insert (times_.end(), t.begin(), t.end()); + times_.insert(times_.end(), t.begin(), t.end()); contacts_.insert(contacts_.end(), c.begin(), c.end()); } } -GaitGenerator::GaitInfo -GaitGenerator::RemoveTransition (const GaitInfo& g) const +GaitGenerator::GaitInfo GaitGenerator::RemoveTransition(const GaitInfo& g) const { GaitInfo new_gait = g; @@ -144,5 +144,3 @@ GaitGenerator::RemoveTransition (const GaitInfo& g) const } } /* namespace towr */ - - diff --git a/towr/src/height_map.cc b/towr/src/height_map.cc index d5883e4e0..fc352d808 100644 --- a/towr/src/height_map.cc +++ b/towr/src/height_map.cc @@ -27,139 +27,165 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include #include +#include #include namespace towr { -HeightMap::Ptr -HeightMap::MakeTerrain (TerrainID type) +HeightMap::Ptr HeightMap::MakeTerrain(TerrainID type) { switch (type) { - case FlatID: return std::make_shared(); break; - case BlockID: return std::make_shared(); break; - case StairsID: return std::make_shared(); break; - case GapID: return std::make_shared(); break; - case SlopeID: return std::make_shared(); break; - case ChimneyID: return std::make_shared(); break; - case ChimneyLRID: return std::make_shared(); break; - default: assert(false); break; + case FlatID: + return std::make_shared(); + break; + case BlockID: + return std::make_shared(); + break; + case StairsID: + return std::make_shared(); + break; + case GapID: + return std::make_shared(); + break; + case SlopeID: + return std::make_shared(); + break; + case ChimneyID: + return std::make_shared(); + break; + case ChimneyLRID: + return std::make_shared(); + break; + default: + assert(false); + break; } } -double -HeightMap::GetDerivativeOfHeightWrt (Dim2D dim, double x, double y) const +double HeightMap::GetDerivativeOfHeightWrt(Dim2D dim, double x, double y) const { switch (dim) { - case X: return GetHeightDerivWrtX(x,y); - case Y: return GetHeightDerivWrtY(x,y); - default: assert(false); // derivative dimension not implemented + case X: + return GetHeightDerivWrtX(x, y); + case Y: + return GetHeightDerivWrtY(x, y); + default: + assert(false); // derivative dimension not implemented } } -HeightMap::Vector3d -HeightMap::GetNormalizedBasis (Direction basis, double x, double y) const +HeightMap::Vector3d HeightMap::GetNormalizedBasis(Direction basis, double x, + double y) const { return GetBasis(basis, x, y).normalized(); } -HeightMap::Vector3d -HeightMap::GetBasis (Direction basis, double x, double y, - const DimDerivs& deriv) const +HeightMap::Vector3d HeightMap::GetBasis(Direction basis, double x, double y, + const DimDerivs& deriv) const { switch (basis) { - case Normal: return GetNormal(x,y, deriv); - case Tangent1: return GetTangent1(x,y, deriv); - case Tangent2: return GetTangent2(x,y, deriv); - default: assert(false); // basis does not exist + case Normal: + return GetNormal(x, y, deriv); + case Tangent1: + return GetTangent1(x, y, deriv); + case Tangent2: + return GetTangent2(x, y, deriv); + default: + assert(false); // basis does not exist } } -HeightMap::Vector3d -HeightMap::GetDerivativeOfNormalizedBasisWrt (Direction basis, Dim2D dim, - double x, double y) const +HeightMap::Vector3d HeightMap::GetDerivativeOfNormalizedBasisWrt( + Direction basis, Dim2D dim, double x, double y) const { // inner derivative Vector3d dv_wrt_dim = GetBasis(basis, x, y, {dim}); // outer derivative - Vector3d v = GetBasis(basis, x,y, {}); - Vector3d dn_norm_wrt_n = GetDerivativeOfNormalizedVectorWrtNonNormalizedIndex(v, dim); + Vector3d v = GetBasis(basis, x, y, {}); + Vector3d dn_norm_wrt_n = + GetDerivativeOfNormalizedVectorWrtNonNormalizedIndex(v, dim); return dn_norm_wrt_n.cwiseProduct(dv_wrt_dim); } -HeightMap::Vector3d -HeightMap::GetNormal(double x, double y, const DimDerivs& deriv) const +HeightMap::Vector3d HeightMap::GetNormal(double x, double y, + const DimDerivs& deriv) const { Vector3d n; bool basis_requested = deriv.empty(); - for (auto dim : {X_,Y_}) { + for (auto dim : {X_, Y_}) { if (basis_requested) n(dim) = -GetDerivativeOfHeightWrt(dim, x, y); else n(dim) = -GetSecondDerivativeOfHeightWrt(dim, deriv.front(), x, y); } - n(Z) = basis_requested? 1.0 : 0.0; + n(Z) = basis_requested ? 1.0 : 0.0; return n; } -HeightMap::Vector3d -HeightMap::GetTangent1 (double x, double y, const DimDerivs& deriv) const +HeightMap::Vector3d HeightMap::GetTangent1(double x, double y, + const DimDerivs& deriv) const { Vector3d tx; bool basis_requested = deriv.empty(); - tx(X) = basis_requested? 1.0 : 0.0; + tx(X) = basis_requested ? 1.0 : 0.0; tx(Y) = 0.0; - tx(Z) = basis_requested? GetDerivativeOfHeightWrt(X_, x, y) - : GetSecondDerivativeOfHeightWrt(X_, deriv.front(), x, y); + tx(Z) = basis_requested + ? GetDerivativeOfHeightWrt(X_, x, y) + : GetSecondDerivativeOfHeightWrt(X_, deriv.front(), x, y); return tx; } -HeightMap::Vector3d -HeightMap::GetTangent2 (double x, double y, const DimDerivs& deriv) const +HeightMap::Vector3d HeightMap::GetTangent2(double x, double y, + const DimDerivs& deriv) const { Vector3d ty; bool basis_requested = deriv.empty(); ty(X) = 0.0; - ty(Y) = basis_requested? 1.0 : 0.0; - ty(Z) = basis_requested? GetDerivativeOfHeightWrt(Y_, x,y) - : GetSecondDerivativeOfHeightWrt(Y_, deriv.front(), x, y); + ty(Y) = basis_requested ? 1.0 : 0.0; + ty(Z) = basis_requested + ? GetDerivativeOfHeightWrt(Y_, x, y) + : GetSecondDerivativeOfHeightWrt(Y_, deriv.front(), x, y); return ty; } HeightMap::Vector3d -HeightMap::GetDerivativeOfNormalizedVectorWrtNonNormalizedIndex ( +HeightMap::GetDerivativeOfNormalizedVectorWrtNonNormalizedIndex( const Vector3d& v, int idx) const { // see notebook or // http://blog.mmacklin.com/2012/05/ - return 1/v.squaredNorm()*(v.norm() * Vector3d::Unit(idx) - v(idx)*v.normalized()); + return 1 / v.squaredNorm() * + (v.norm() * Vector3d::Unit(idx) - v(idx) * v.normalized()); } -double -HeightMap::GetSecondDerivativeOfHeightWrt (Dim2D dim1, Dim2D dim2, - double x, double y) const +double HeightMap::GetSecondDerivativeOfHeightWrt(Dim2D dim1, Dim2D dim2, + double x, double y) const { if (dim1 == X_) { - if (dim2 == X_) return GetHeightDerivWrtXX(x,y); - if (dim2 == Y_) return GetHeightDerivWrtXY(x,y); + if (dim2 == X_) + return GetHeightDerivWrtXX(x, y); + if (dim2 == Y_) + return GetHeightDerivWrtXY(x, y); } else { - if (dim2 == X_) return GetHeightDerivWrtYX(x,y); - if (dim2 == Y_) return GetHeightDerivWrtYY(x,y); + if (dim2 == X_) + return GetHeightDerivWrtYX(x, y); + if (dim2 == Y_) + return GetHeightDerivWrtYY(x, y); } - assert(false); // second derivative not specified. + assert(false); // second derivative not specified. } } /* namespace towr */ diff --git a/towr/src/height_map_examples.cc b/towr/src/height_map_examples.cc index ce695f012..52f7449f7 100644 --- a/towr/src/height_map_examples.cc +++ b/towr/src/height_map_examples.cc @@ -31,106 +31,95 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { - FlatGround::FlatGround(double height) { height_ = height; } -double -Block::GetHeight (double x, double y) const +double Block::GetHeight(double x, double y) const { double h = 0.0; // very steep ramp leading up to block - if (block_start <= x && x <=block_start+eps_) - h = slope_*(x-block_start); + if (block_start <= x && x <= block_start + eps_) + h = slope_ * (x - block_start); - if (block_start+eps_ <= x && x <= block_start+length_) + if (block_start + eps_ <= x && x <= block_start + length_) h = height_; return h; } -double -Block::GetHeightDerivWrtX (double x, double y) const +double Block::GetHeightDerivWrtX(double x, double y) const { double dhdx = 0.0; // very steep ramp leading up to block - if (block_start <= x && x <=block_start+eps_) + if (block_start <= x && x <= block_start + eps_) dhdx = slope_; return dhdx; } - // STAIRS -double -Stairs::GetHeight (double x, double y) const +double Stairs::GetHeight(double x, double y) const { double h = 0.0; - if (x>=first_step_start_) + if (x >= first_step_start_) h = height_first_step; - if (x>=first_step_start_+first_step_width_) + if (x >= first_step_start_ + first_step_width_) h = height_second_step; - if (x>=first_step_start_+first_step_width_+width_top) + if (x >= first_step_start_ + first_step_width_ + width_top) h = 0.0; return h; } - // GAP -double -Gap::GetHeight (double x, double y) const +double Gap::GetHeight(double x, double y) const { double h = 0.0; // modelled as parabola if (gap_start_ <= x && x <= gap_end_x) - h = a*x*x + b*x + c; + h = a * x * x + b * x + c; return h; } -double -Gap::GetHeightDerivWrtX (double x, double y) const +double Gap::GetHeightDerivWrtX(double x, double y) const { double dhdx = 0.0; if (gap_start_ <= x && x <= gap_end_x) - dhdx = 2*a*x + b; + dhdx = 2 * a * x + b; return dhdx; } -double -Gap::GetHeightDerivWrtXX (double x, double y) const +double Gap::GetHeightDerivWrtXX(double x, double y) const { double dzdxx = 0.0; if (gap_start_ <= x && x <= gap_end_x) - dzdxx = 2*a; + dzdxx = 2 * a; return dzdxx; } - // SLOPE -double -Slope::GetHeight (double x, double y) const +double Slope::GetHeight(double x, double y) const { double z = 0.0; if (x >= slope_start_) - z = slope_*(x-slope_start_); + z = slope_ * (x - slope_start_); // going back down if (x >= x_down_start_) { - z = height_center - slope_*(x-x_down_start_); + z = height_center - slope_ * (x - x_down_start_); } // back on flat ground @@ -140,8 +129,7 @@ Slope::GetHeight (double x, double y) const return z; } -double -Slope::GetHeightDerivWrtX (double x, double y) const +double Slope::GetHeightDerivWrtX(double x, double y) const { double dzdx = 0.0; if (x >= slope_start_) @@ -156,55 +144,49 @@ Slope::GetHeightDerivWrtX (double x, double y) const return dzdx; } - // Chimney -double -Chimney::GetHeight (double x, double y) const +double Chimney::GetHeight(double x, double y) const { double z = 0.0; - if (x_start_<=x && x<=x_end_) - z = slope_*(y-y_start_); + if (x_start_ <= x && x <= x_end_) + z = slope_ * (y - y_start_); return z; } -double -Chimney::GetHeightDerivWrtY (double x, double y) const +double Chimney::GetHeightDerivWrtY(double x, double y) const { double dzdy = 0.0; - if (x_start_<= x && x<= x_end_) + if (x_start_ <= x && x <= x_end_) dzdy = slope_; return dzdy; } - // Chimney LR -double -ChimneyLR::GetHeight (double x, double y) const +double ChimneyLR::GetHeight(double x, double y) const { double z = 0.0; - if (x_start_<=x && x<=x_end1_) - z = slope_*(y-y_start_); + if (x_start_ <= x && x <= x_end1_) + z = slope_ * (y - y_start_); - if (x_end1_<=x && x<=x_end2_) - z = -slope_*(y+y_start_); + if (x_end1_ <= x && x <= x_end2_) + z = -slope_ * (y + y_start_); return z; } -double -ChimneyLR::GetHeightDerivWrtY (double x, double y) const +double ChimneyLR::GetHeightDerivWrtY(double x, double y) const { double dzdy = 0.0; if (x_start_ <= x && x <= x_end1_) dzdy = slope_; - if (x_end1_<=x && x<=x_end2_) + if (x_end1_ <= x && x <= x_end2_) dzdy = -slope_; return dzdy; diff --git a/towr/src/linear_constraint.cc b/towr/src/linear_constraint.cc index 240d02876..e68c77302 100644 --- a/towr/src/linear_constraint.cc +++ b/towr/src/linear_constraint.cc @@ -31,40 +31,36 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { - -LinearEqualityConstraint::LinearEqualityConstraint ( - const Eigen::MatrixXd& M, - const Eigen::VectorXd& v, - const std::string& variable_name) +LinearEqualityConstraint::LinearEqualityConstraint( + const Eigen::MatrixXd& M, const Eigen::VectorXd& v, + const std::string& variable_name) : ConstraintSet(v.rows(), "linear-equality-" + variable_name) { - M_ = M; - v_ = v; - variable_name_ = variable_name; + M_ = M; + v_ = v; + variable_name_ = variable_name; } -LinearEqualityConstraint::VectorXd -LinearEqualityConstraint::GetValues () const +LinearEqualityConstraint::VectorXd LinearEqualityConstraint::GetValues() const { VectorXd x = GetVariables()->GetComponent(variable_name_)->GetValues(); - return M_*x; + return M_ * x; } -LinearEqualityConstraint::VecBound -LinearEqualityConstraint::GetBounds () const +LinearEqualityConstraint::VecBound LinearEqualityConstraint::GetBounds() const { VecBound bounds; - for (int i=0; i -#include #include +#include #include #include #include #include +#include #include #include #include -#include #include #include @@ -48,7 +48,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { -NlpFormulation::NlpFormulation () +NlpFormulation::NlpFormulation() { using namespace std; cout << "\n"; @@ -60,8 +60,8 @@ NlpFormulation::NlpFormulation () cout << "\n\n"; } -NlpFormulation::VariablePtrVec -NlpFormulation::GetVariableSets (SplineHolder& spline_holder) +NlpFormulation::VariablePtrVec NlpFormulation::GetVariableSets( + SplineHolder& spline_holder) { VariablePtrVec vars; @@ -82,123 +82,127 @@ NlpFormulation::GetVariableSets (SplineHolder& spline_holder) } // stores these readily constructed spline - spline_holder = SplineHolder(base_motion.at(0), // linear - base_motion.at(1), // angular - params_.GetBasePolyDurations(), - ee_motion, - ee_force, - contact_schedule, - params_.IsOptimizeTimings()); + spline_holder = + SplineHolder(base_motion.at(0), // linear + base_motion.at(1), // angular + params_.GetBasePolyDurations(), ee_motion, ee_force, + contact_schedule, params_.IsOptimizeTimings()); return vars; } -std::vector -NlpFormulation::MakeBaseVariables () const +std::vector NlpFormulation::MakeBaseVariables() const { std::vector vars; int n_nodes = params_.GetBasePolyDurations().size() + 1; - auto spline_lin = std::make_shared(n_nodes, k3D, id::base_lin_nodes); + auto spline_lin = + std::make_shared(n_nodes, k3D, id::base_lin_nodes); double x = final_base_.lin.p().x(); double y = final_base_.lin.p().y(); - double z = terrain_->GetHeight(x,y) - model_.kinematic_model_->GetNominalStanceInBase().front().z(); + double z = terrain_->GetHeight(x, y) - + model_.kinematic_model_->GetNominalStanceInBase().front().z(); Vector3d final_pos(x, y, z); - spline_lin->SetByLinearInterpolation(initial_base_.lin.p(), final_pos, params_.GetTotalTime()); - spline_lin->AddStartBound(kPos, {X,Y,Z}, initial_base_.lin.p()); - spline_lin->AddStartBound(kVel, {X,Y,Z}, initial_base_.lin.v()); - spline_lin->AddFinalBound(kPos, params_.bounds_final_lin_pos_, final_base_.lin.p()); - spline_lin->AddFinalBound(kVel, params_.bounds_final_lin_vel_, final_base_.lin.v()); + spline_lin->SetByLinearInterpolation(initial_base_.lin.p(), final_pos, + params_.GetTotalTime()); + spline_lin->AddStartBound(kPos, {X, Y, Z}, initial_base_.lin.p()); + spline_lin->AddStartBound(kVel, {X, Y, Z}, initial_base_.lin.v()); + spline_lin->AddFinalBound(kPos, params_.bounds_final_lin_pos_, + final_base_.lin.p()); + spline_lin->AddFinalBound(kVel, params_.bounds_final_lin_vel_, + final_base_.lin.v()); vars.push_back(spline_lin); - auto spline_ang = std::make_shared(n_nodes, k3D, id::base_ang_nodes); - spline_ang->SetByLinearInterpolation(initial_base_.ang.p(), final_base_.ang.p(), params_.GetTotalTime()); - spline_ang->AddStartBound(kPos, {X,Y,Z}, initial_base_.ang.p()); - spline_ang->AddStartBound(kVel, {X,Y,Z}, initial_base_.ang.v()); - spline_ang->AddFinalBound(kPos, params_.bounds_final_ang_pos_, final_base_.ang.p()); - spline_ang->AddFinalBound(kVel, params_.bounds_final_ang_vel_, final_base_.ang.v()); + auto spline_ang = + std::make_shared(n_nodes, k3D, id::base_ang_nodes); + spline_ang->SetByLinearInterpolation( + initial_base_.ang.p(), final_base_.ang.p(), params_.GetTotalTime()); + spline_ang->AddStartBound(kPos, {X, Y, Z}, initial_base_.ang.p()); + spline_ang->AddStartBound(kVel, {X, Y, Z}, initial_base_.ang.v()); + spline_ang->AddFinalBound(kPos, params_.bounds_final_ang_pos_, + final_base_.ang.p()); + spline_ang->AddFinalBound(kVel, params_.bounds_final_ang_vel_, + final_base_.ang.v()); vars.push_back(spline_ang); return vars; } std::vector -NlpFormulation::MakeEndeffectorVariables () const +NlpFormulation::MakeEndeffectorVariables() const { std::vector vars; // Endeffector Motions double T = params_.GetTotalTime(); - for (int ee=0; ee( - params_.GetPhaseCount(ee), - params_.ee_in_contact_at_start_.at(ee), - id::EEMotionNodes(ee), - params_.ee_polynomials_per_swing_phase_); + params_.GetPhaseCount(ee), params_.ee_in_contact_at_start_.at(ee), + id::EEMotionNodes(ee), params_.ee_polynomials_per_swing_phase_); // initialize towards final footholds double yaw = final_base_.ang.p().z(); Eigen::Vector3d euler(0.0, 0.0, yaw); Eigen::Matrix3d w_R_b = EulerConverter::GetRotationMatrixBaseToWorld(euler); - Vector3d final_ee_pos_W = final_base_.lin.p() + w_R_b*model_.kinematic_model_->GetNominalStanceInBase().at(ee); + Vector3d final_ee_pos_W = + final_base_.lin.p() + + w_R_b * model_.kinematic_model_->GetNominalStanceInBase().at(ee); double x = final_ee_pos_W.x(); double y = final_ee_pos_W.y(); - double z = terrain_->GetHeight(x,y); - nodes->SetByLinearInterpolation(initial_ee_W_.at(ee), Vector3d(x,y,z), T); + double z = terrain_->GetHeight(x, y); + nodes->SetByLinearInterpolation(initial_ee_W_.at(ee), Vector3d(x, y, z), T); - nodes->AddStartBound(kPos, {X,Y,Z}, initial_ee_W_.at(ee)); + nodes->AddStartBound(kPos, {X, Y, Z}, initial_ee_W_.at(ee)); vars.push_back(nodes); } return vars; } -std::vector -NlpFormulation::MakeForceVariables () const +std::vector NlpFormulation::MakeForceVariables() + const { std::vector vars; double T = params_.GetTotalTime(); - for (int ee=0; ee( - params_.GetPhaseCount(ee), - params_.ee_in_contact_at_start_.at(ee), - id::EEForceNodes(ee), - params_.force_polynomials_per_stance_phase_); + params_.GetPhaseCount(ee), params_.ee_in_contact_at_start_.at(ee), + id::EEForceNodes(ee), params_.force_polynomials_per_stance_phase_); // initialize with mass of robot distributed equally on all legs double m = model_.dynamic_model_->m(); double g = model_.dynamic_model_->g(); - Vector3d f_stance(0.0, 0.0, m*g/params_.GetEECount()); - nodes->SetByLinearInterpolation(f_stance, f_stance, T); // stay constant + Vector3d f_stance(0.0, 0.0, m * g / params_.GetEECount()); + nodes->SetByLinearInterpolation(f_stance, f_stance, T); // stay constant vars.push_back(nodes); } return vars; } -std::vector -NlpFormulation::MakeContactScheduleVariables () const +std::vector NlpFormulation::MakeContactScheduleVariables() + const { std::vector vars; - for (int ee=0; ee(ee, - params_.ee_phase_durations_.at(ee), - params_.ee_in_contact_at_start_.at(ee), - params_.bound_phase_duration_.first, - params_.bound_phase_duration_.second); + for (int ee = 0; ee < params_.GetEECount(); ee++) { + auto var = + std::make_shared(ee, params_.ee_phase_durations_.at(ee), + params_.ee_in_contact_at_start_.at(ee), + params_.bound_phase_duration_.first, + params_.bound_phase_duration_.second); vars.push_back(var); } return vars; } -NlpFormulation::ContraintPtrVec -NlpFormulation::GetConstraints(const SplineHolder& spline_holder) const +NlpFormulation::ContraintPtrVec NlpFormulation::GetConstraints( + const SplineHolder& spline_holder) const { ContraintPtrVec constraints; for (auto name : params_.constraints_) @@ -208,66 +212,68 @@ NlpFormulation::GetConstraints(const SplineHolder& spline_holder) const return constraints; } -NlpFormulation::ContraintPtrVec -NlpFormulation::GetConstraint (Parameters::ConstraintName name, - const SplineHolder& s) const +NlpFormulation::ContraintPtrVec NlpFormulation::GetConstraint( + Parameters::ConstraintName name, const SplineHolder& s) const { switch (name) { - case Parameters::Dynamic: return MakeDynamicConstraint(s); - case Parameters::EndeffectorRom: return MakeRangeOfMotionBoxConstraint(s); - case Parameters::BaseRom: return MakeBaseRangeOfMotionConstraint(s); - case Parameters::TotalTime: return MakeTotalTimeConstraint(); - case Parameters::Terrain: return MakeTerrainConstraint(); - case Parameters::Force: return MakeForceConstraint(); - case Parameters::Swing: return MakeSwingConstraint(); - case Parameters::BaseAcc: return MakeBaseAccConstraint(s); - default: throw std::runtime_error("constraint not defined!"); + case Parameters::Dynamic: + return MakeDynamicConstraint(s); + case Parameters::EndeffectorRom: + return MakeRangeOfMotionBoxConstraint(s); + case Parameters::BaseRom: + return MakeBaseRangeOfMotionConstraint(s); + case Parameters::TotalTime: + return MakeTotalTimeConstraint(); + case Parameters::Terrain: + return MakeTerrainConstraint(); + case Parameters::Force: + return MakeForceConstraint(); + case Parameters::Swing: + return MakeSwingConstraint(); + case Parameters::BaseAcc: + return MakeBaseAccConstraint(s); + default: + throw std::runtime_error("constraint not defined!"); } } - -NlpFormulation::ContraintPtrVec -NlpFormulation::MakeBaseRangeOfMotionConstraint (const SplineHolder& s) const +NlpFormulation::ContraintPtrVec NlpFormulation::MakeBaseRangeOfMotionConstraint( + const SplineHolder& s) const { - return {std::make_shared(params_.GetTotalTime(), - params_.dt_constraint_base_motion_, - s)}; + return {std::make_shared( + params_.GetTotalTime(), params_.dt_constraint_base_motion_, s)}; } -NlpFormulation::ContraintPtrVec -NlpFormulation::MakeDynamicConstraint(const SplineHolder& s) const +NlpFormulation::ContraintPtrVec NlpFormulation::MakeDynamicConstraint( + const SplineHolder& s) const { - auto constraint = std::make_shared(model_.dynamic_model_, - params_.GetTotalTime(), - params_.dt_constraint_dynamic_, - s); + auto constraint = std::make_shared( + model_.dynamic_model_, params_.GetTotalTime(), + params_.dt_constraint_dynamic_, s); return {constraint}; } -NlpFormulation::ContraintPtrVec -NlpFormulation::MakeRangeOfMotionBoxConstraint (const SplineHolder& s) const +NlpFormulation::ContraintPtrVec NlpFormulation::MakeRangeOfMotionBoxConstraint( + const SplineHolder& s) const { ContraintPtrVec c; - for (int ee=0; ee(model_.kinematic_model_, - params_.GetTotalTime(), - params_.dt_constraint_range_of_motion_, - ee, - s); + for (int ee = 0; ee < params_.GetEECount(); ee++) { + auto rom = std::make_shared( + model_.kinematic_model_, params_.GetTotalTime(), + params_.dt_constraint_range_of_motion_, ee, s); c.push_back(rom); } return c; } -NlpFormulation::ContraintPtrVec -NlpFormulation::MakeTotalTimeConstraint () const +NlpFormulation::ContraintPtrVec NlpFormulation::MakeTotalTimeConstraint() const { ContraintPtrVec c; double T = params_.GetTotalTime(); - for (int ee=0; ee(T, ee); c.push_back(duration_constraint); } @@ -275,40 +281,37 @@ NlpFormulation::MakeTotalTimeConstraint () const return c; } -NlpFormulation::ContraintPtrVec -NlpFormulation::MakeTerrainConstraint () const +NlpFormulation::ContraintPtrVec NlpFormulation::MakeTerrainConstraint() const { ContraintPtrVec constraints; - for (int ee=0; ee(terrain_, id::EEMotionNodes(ee)); + for (int ee = 0; ee < params_.GetEECount(); ee++) { + auto c = + std::make_shared(terrain_, id::EEMotionNodes(ee)); constraints.push_back(c); } return constraints; } -NlpFormulation::ContraintPtrVec -NlpFormulation::MakeForceConstraint () const +NlpFormulation::ContraintPtrVec NlpFormulation::MakeForceConstraint() const { ContraintPtrVec constraints; - for (int ee=0; ee(terrain_, - params_.force_limit_in_normal_direction_, - ee); + for (int ee = 0; ee < params_.GetEECount(); ee++) { + auto c = std::make_shared( + terrain_, params_.force_limit_in_normal_direction_, ee); constraints.push_back(c); } return constraints; } -NlpFormulation::ContraintPtrVec -NlpFormulation::MakeSwingConstraint () const +NlpFormulation::ContraintPtrVec NlpFormulation::MakeSwingConstraint() const { ContraintPtrVec constraints; - for (int ee=0; ee(id::EEMotionNodes(ee)); constraints.push_back(swing); } @@ -316,22 +319,21 @@ NlpFormulation::MakeSwingConstraint () const return constraints; } -NlpFormulation::ContraintPtrVec -NlpFormulation::MakeBaseAccConstraint (const SplineHolder& s) const +NlpFormulation::ContraintPtrVec NlpFormulation::MakeBaseAccConstraint( + const SplineHolder& s) const { ContraintPtrVec constraints; - constraints.push_back(std::make_shared - (s.base_linear_, id::base_lin_nodes)); + constraints.push_back(std::make_shared( + s.base_linear_, id::base_lin_nodes)); - constraints.push_back(std::make_shared - (s.base_angular_, id::base_ang_nodes)); + constraints.push_back(std::make_shared( + s.base_angular_, id::base_ang_nodes)); return constraints; } -NlpFormulation::ContraintPtrVec -NlpFormulation::GetCosts() const +NlpFormulation::ContraintPtrVec NlpFormulation::GetCosts() const { ContraintPtrVec costs; for (const auto& pair : params_.costs_) @@ -341,35 +343,39 @@ NlpFormulation::GetCosts() const return costs; } -NlpFormulation::CostPtrVec -NlpFormulation::GetCost(const Parameters::CostName& name, double weight) const +NlpFormulation::CostPtrVec NlpFormulation::GetCost( + const Parameters::CostName& name, double weight) const { switch (name) { - case Parameters::ForcesCostID: return MakeForcesCost(weight); - case Parameters::EEMotionCostID: return MakeEEMotionCost(weight); - default: throw std::runtime_error("cost not defined!"); + case Parameters::ForcesCostID: + return MakeForcesCost(weight); + case Parameters::EEMotionCostID: + return MakeEEMotionCost(weight); + default: + throw std::runtime_error("cost not defined!"); } } -NlpFormulation::CostPtrVec -NlpFormulation::MakeForcesCost(double weight) const +NlpFormulation::CostPtrVec NlpFormulation::MakeForcesCost(double weight) const { CostPtrVec cost; - for (int ee=0; ee(id::EEForceNodes(ee), kPos, Z, weight)); + for (int ee = 0; ee < params_.GetEECount(); ee++) + cost.push_back( + std::make_shared(id::EEForceNodes(ee), kPos, Z, weight)); return cost; } -NlpFormulation::CostPtrVec -NlpFormulation::MakeEEMotionCost(double weight) const +NlpFormulation::CostPtrVec NlpFormulation::MakeEEMotionCost(double weight) const { CostPtrVec cost; - for (int ee=0; ee(id::EEMotionNodes(ee), kVel, X, weight)); - cost.push_back(std::make_shared(id::EEMotionNodes(ee), kVel, Y, weight)); + for (int ee = 0; ee < params_.GetEECount(); ee++) { + cost.push_back( + std::make_shared(id::EEMotionNodes(ee), kVel, X, weight)); + cost.push_back( + std::make_shared(id::EEMotionNodes(ee), kVel, Y, weight)); } return cost; diff --git a/towr/src/node_cost.cc b/towr/src/node_cost.cc index c0a482199..e0d136723 100644 --- a/towr/src/node_cost.cc +++ b/towr/src/node_cost.cc @@ -33,47 +33,43 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { -NodeCost::NodeCost (const std::string& nodes_id, Dx deriv, int dim, double weight) - : CostTerm(nodes_id - +"-dx_"+std::to_string(deriv) - +"-dim_"+std::to_string(dim)) +NodeCost::NodeCost(const std::string& nodes_id, Dx deriv, int dim, + double weight) + : CostTerm(nodes_id + "-dx_" + std::to_string(deriv) + "-dim_" + + std::to_string(dim)) { node_id_ = nodes_id; - deriv_ = deriv; - dim_ = dim; - weight_ = weight; + deriv_ = deriv; + dim_ = dim; + weight_ = weight; } -void -NodeCost::InitVariableDependedQuantities (const VariablesPtr& x) +void NodeCost::InitVariableDependedQuantities(const VariablesPtr& x) { nodes_ = x->GetComponent(node_id_); } -double -NodeCost::GetCost () const +double NodeCost::GetCost() const { double cost; for (auto n : nodes_->GetNodes()) { double val = n.at(deriv_)(dim_); - cost += weight_*std::pow(val,2); + cost += weight_ * std::pow(val, 2); } return cost; } -void -NodeCost::FillJacobianBlock (std::string var_set, Jacobian& jac) const +void NodeCost::FillJacobianBlock(std::string var_set, Jacobian& jac) const { if (var_set == node_id_) { - for (int i=0; iGetRows(); ++i) + for (int i = 0; i < nodes_->GetRows(); ++i) for (auto nvi : nodes_->GetNodeValuesInfo(i)) - if (nvi.deriv_==deriv_ && nvi.dim_==dim_) { + if (nvi.deriv_ == deriv_ && nvi.dim_ == dim_) { double val = nodes_->GetNodes().at(nvi.id_).at(deriv_)(dim_); - jac.coeffRef(0, i) += weight_*2.0*val; + jac.coeffRef(0, i) += weight_ * 2.0 * val; } } } } /* namespace towr */ - diff --git a/towr/src/node_spline.cc b/towr/src/node_spline.cc index 5d149f0ad..c8e3f93f6 100644 --- a/towr/src/node_spline.cc +++ b/towr/src/node_spline.cc @@ -35,17 +35,17 @@ namespace towr { NodeSpline::NodeSpline(NodeSubjectPtr const node_variables, const VecTimes& polynomial_durations) - : Spline(polynomial_durations, node_variables->GetDim()), - NodesObserver(node_variables) + : Spline(polynomial_durations, node_variables->GetDim()), + NodesObserver(node_variables) { UpdateNodes(); - jac_wrt_nodes_structure_ = Jacobian(node_variables->GetDim(), node_variables->GetRows()); + jac_wrt_nodes_structure_ = + Jacobian(node_variables->GetDim(), node_variables->GetRows()); } -void -NodeSpline::UpdateNodes () +void NodeSpline::UpdateNodes() { - for (int i=0; iGetBoundaryNodes(i); cubic_polys_.at(i).SetNodes(nodes.front(), nodes.back()); } @@ -53,23 +53,23 @@ NodeSpline::UpdateNodes () UpdatePolynomialCoeff(); } -int -NodeSpline::GetNodeVariablesCount() const +int NodeSpline::GetNodeVariablesCount() const { return node_values_->GetRows(); } -NodeSpline::Jacobian -NodeSpline::GetJacobianWrtNodes (double t_global, Dx dxdt) const +NodeSpline::Jacobian NodeSpline::GetJacobianWrtNodes(double t_global, + Dx dxdt) const { - int id; double t_local; + int id; + double t_local; std::tie(id, t_local) = GetLocalTime(t_global, GetPolyDurations()); return GetJacobianWrtNodes(id, t_local, dxdt); } -NodeSpline::Jacobian -NodeSpline::GetJacobianWrtNodes (int id, double t_local, Dx dxdt) const +NodeSpline::Jacobian NodeSpline::GetJacobianWrtNodes(int id, double t_local, + Dx dxdt) const { Jacobian jac = jac_wrt_nodes_structure_; FillJacobianWrtNodes(id, t_local, dxdt, jac, false); @@ -81,24 +81,27 @@ NodeSpline::GetJacobianWrtNodes (int id, double t_local, Dx dxdt) const return jac; } -void -NodeSpline::FillJacobianWrtNodes (int poly_id, double t_local, Dx dxdt, - Jacobian& jac, bool fill_with_zeros) const +void NodeSpline::FillJacobianWrtNodes(int poly_id, double t_local, Dx dxdt, + Jacobian& jac, bool fill_with_zeros) const { - for (int idx=0; idxGetNodeValuesInfo(idx)) { - for (auto side : {NodesVariables::Side::Start, NodesVariables::Side::End}) { // every jacobian is affected by two nodes + // every jacobian is affected by two nodes + for (auto side : + {NodesVariables::Side::Start, NodesVariables::Side::End}) { int node = node_values_->GetNodeId(poly_id, side); if (node == nvi.id_) { double val = 0.0; if (side == NodesVariables::Side::Start) - val = cubic_polys_.at(poly_id).GetDerivativeWrtStartNode(dxdt, nvi.deriv_, t_local); + val = cubic_polys_.at(poly_id).GetDerivativeWrtStartNode( + dxdt, nvi.deriv_, t_local); else if (side == NodesVariables::Side::End) - val = cubic_polys_.at(poly_id).GetDerivativeWrtEndNode(dxdt, nvi.deriv_, t_local); + val = cubic_polys_.at(poly_id).GetDerivativeWrtEndNode( + dxdt, nvi.deriv_, t_local); else - assert(false); // this shouldn't happen + assert(false); // this shouldn't happen // if only want structure if (fill_with_zeros) diff --git a/towr/src/nodes_observer.cc b/towr/src/nodes_observer.cc index 9ce4f2f18..80326865a 100644 --- a/towr/src/nodes_observer.cc +++ b/towr/src/nodes_observer.cc @@ -40,6 +40,4 @@ NodesObserver::NodesObserver(NodeSubjectPtr subject) subject->AddObserver(this); }; -} // namespace towr - - +} // namespace towr diff --git a/towr/src/nodes_variables.cc b/towr/src/nodes_variables.cc index b298bdd13..4aba5e865 100644 --- a/towr/src/nodes_variables.cc +++ b/towr/src/nodes_variables.cc @@ -31,67 +31,58 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { - -NodesVariables::NodesVariables (const std::string& name) +NodesVariables::NodesVariables(const std::string& name) : VariableSet(kSpecifyLater, name) -{ -} +{} -int -NodesVariables::GetOptIndex(const NodeValueInfo& nvi_des) const +int NodesVariables::GetOptIndex(const NodeValueInfo& nvi_des) const { // could also cache this as map for more efficiency, but adding complexity - for (int idx=0; idxUpdateNodes(); } -void -NodesVariables::AddObserver(ObserverPtr const o) +void NodesVariables::AddObserver(ObserverPtr const o) { - observers_.push_back(o); + observers_.push_back(o); } -int -NodesVariables::GetNodeId (int poly_id, Side side) +int NodesVariables::GetNodeId(int poly_id, Side side) { return poly_id + side; } -const std::vector -NodesVariables::GetBoundaryNodes(int poly_id) const +const std::vector NodesVariables::GetBoundaryNodes(int poly_id) const { std::vector nodes; nodes.push_back(nodes_.at(GetNodeId(poly_id, Side::Start))); @@ -99,46 +90,42 @@ NodesVariables::GetBoundaryNodes(int poly_id) const return nodes; } -int -NodesVariables::GetDim() const +int NodesVariables::GetDim() const { return n_dim_; } -int -NodesVariables::GetPolynomialCount() const +int NodesVariables::GetPolynomialCount() const { return nodes_.size() - 1; } -NodesVariables::VecBound -NodesVariables::GetBounds () const +NodesVariables::VecBound NodesVariables::GetBounds() const { return bounds_; } -const std::vector -NodesVariables::GetNodes() const +const std::vector NodesVariables::GetNodes() const { return nodes_; } -void -NodesVariables::SetByLinearInterpolation(const VectorXd& initial_val, - const VectorXd& final_val, - double t_total) +void NodesVariables::SetByLinearInterpolation(const VectorXd& initial_val, + const VectorXd& final_val, + double t_total) { // only set those that are part of optimization variables, // do not overwrite phase-based parameterization - VectorXd dp = final_val-initial_val; + VectorXd dp = final_val - initial_val; VectorXd average_velocity = dp / t_total; - int num_nodes = nodes_.size(); + int num_nodes = nodes_.size(); - for (int idx=0; idx(num_nodes-1)*dp; + VectorXd pos = + initial_val + nvi.id_ / static_cast(num_nodes - 1) * dp; nodes_.at(nvi.id_).at(kPos)(nvi.dim_) = pos(nvi.dim_); } @@ -149,50 +136,45 @@ NodesVariables::SetByLinearInterpolation(const VectorXd& initial_val, } } -void -NodesVariables::AddBounds(int node_id, Dx deriv, - const std::vector& dimensions, - const VectorXd& val) +void NodesVariables::AddBounds(int node_id, Dx deriv, + const std::vector& dimensions, + const VectorXd& val) { for (auto dim : dimensions) AddBound(NodeValueInfo(node_id, deriv, dim), val(dim)); } -void -NodesVariables::AddBound (const NodeValueInfo& nvi_des, double val) +void NodesVariables::AddBound(const NodeValueInfo& nvi_des, double val) { - for (int idx=0; idx& dimensions, const VectorXd& val) +void NodesVariables::AddStartBound(Dx d, const std::vector& dimensions, + const VectorXd& val) { AddBounds(0, d, dimensions, val); } -void -NodesVariables::AddFinalBound (Dx deriv, const std::vector& dimensions, - const VectorXd& val) +void NodesVariables::AddFinalBound(Dx deriv, const std::vector& dimensions, + const VectorXd& val) { - AddBounds(nodes_.size()-1, deriv, dimensions, val); + AddBounds(nodes_.size() - 1, deriv, dimensions, val); } -NodesVariables::NodeValueInfo::NodeValueInfo(int node_id, Dx deriv, int node_dim) +NodesVariables::NodeValueInfo::NodeValueInfo(int node_id, Dx deriv, + int node_dim) { id_ = node_id; deriv_ = deriv; dim_ = node_dim; } -int -NodesVariables::NodeValueInfo::operator==(const NodeValueInfo& right) const +int NodesVariables::NodeValueInfo::operator==(const NodeValueInfo& right) const { - return (id_ == right.id_) - && (deriv_ == right.deriv_) - && (dim_ == right.dim_); + return (id_ == right.id_) && (deriv_ == right.deriv_) && (dim_ == right.dim_); }; } /* namespace towr */ diff --git a/towr/src/nodes_variables_all.cc b/towr/src/nodes_variables_all.cc index ebde2b9dc..d820b10ba 100644 --- a/towr/src/nodes_variables_all.cc +++ b/towr/src/nodes_variables_all.cc @@ -31,29 +31,31 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { -NodesVariablesAll::NodesVariablesAll (int n_nodes, int n_dim, std::string variable_id) +NodesVariablesAll::NodesVariablesAll(int n_nodes, int n_dim, + std::string variable_id) : NodesVariables(variable_id) { - int n_opt_variables = n_nodes*Node::n_derivatives*n_dim; + int n_opt_variables = n_nodes * Node::n_derivatives * n_dim; - n_dim_ = n_dim; + n_dim_ = n_dim; nodes_ = std::vector(n_nodes, Node(n_dim)); bounds_ = VecBound(n_opt_variables, ifopt::NoBound); SetRows(n_opt_variables); } std::vector -NodesVariablesAll::GetNodeValuesInfo (int idx) const +NodesVariablesAll::GetNodeValuesInfo(int idx) const { std::vector vec_nvi; - int n_opt_values_per_node_ = 2*GetDim(); - int internal_id = idx%n_opt_values_per_node_; // 0...6 (p.x, p.y, p.z, v.x, v.y. v.z) + int n_opt_values_per_node_ = 2 * GetDim(); + int internal_id = + idx % n_opt_values_per_node_; // 0...6 (p.x, p.y, p.z, v.x, v.y. v.z) NodeValueInfo nvi; - nvi.deriv_ = internal_id #include +#include #include namespace towr { - -std::vector -BuildPolyInfos (int phase_count, bool first_phase_constant, - int n_polys_in_changing_phase) +std::vector BuildPolyInfos( + int phase_count, bool first_phase_constant, int n_polys_in_changing_phase) { using PolyInfo = NodesVariablesPhaseBased::PolyInfo; std::vector polynomial_info; bool phase_constant = first_phase_constant; - for (int i=0; i(n_nodes, Node(n_dim_)); + n_dim_ = k3D; + int n_nodes = polynomial_info_.size() + 1; + nodes_ = std::vector(n_nodes, Node(n_dim_)); } NodesVariablesPhaseBased::VecDurations -NodesVariablesPhaseBased::ConvertPhaseToPolyDurations(const VecDurations& phase_durations) const +NodesVariablesPhaseBased::ConvertPhaseToPolyDurations( + const VecDurations& phase_durations) const { VecDurations poly_durations; - for (int i=0; i -NodesVariablesPhaseBased::GetAdjacentPolyIds (int node_id) const +std::vector NodesVariablesPhaseBased::GetAdjacentPolyIds(int node_id) const { std::vector poly_ids; - int last_node_id = GetNodes().size()-1; + int last_node_id = GetNodes().size() - 1; - if (node_id==0) + if (node_id == 0) poly_ids.push_back(0); - else if (node_id==last_node_id) - poly_ids.push_back(last_node_id-1); + else if (node_id == last_node_id) + poly_ids.push_back(last_node_id - 1); else { - poly_ids.push_back(node_id-1); + poly_ids.push_back(node_id - 1); poly_ids.push_back(node_id); } @@ -179,16 +175,15 @@ NodesVariablesPhaseBased::GetAdjacentPolyIds (int node_id) const } NodesVariablesPhaseBased::PolyInfo::PolyInfo(int phase, int poly_id_in_phase, - int num_polys_in_phase, bool is_constant) - :phase_(phase), - poly_in_phase_(poly_id_in_phase), - n_polys_in_phase_(num_polys_in_phase), - is_constant_(is_constant) -{ -} - -void -NodesVariablesPhaseBased::SetNumberOfVariables(int n_variables) + int num_polys_in_phase, + bool is_constant) + : phase_(phase), + poly_in_phase_(poly_id_in_phase), + n_polys_in_phase_(num_polys_in_phase), + is_constant_(is_constant) +{} + +void NodesVariablesPhaseBased::SetNumberOfVariables(int n_variables) { bounds_ = VecBound(n_variables, ifopt::NoBound); SetRows(n_variables); @@ -198,25 +193,25 @@ NodesVariablesEEMotion::NodesVariablesEEMotion(int phase_count, bool is_in_contact_at_start, const std::string& name, int n_polys_in_changing_phase) - :NodesVariablesPhaseBased(phase_count, - is_in_contact_at_start, // contact phase for motion is constant - name, - n_polys_in_changing_phase) + : NodesVariablesPhaseBased( + phase_count, + is_in_contact_at_start, // contact phase for motion is constant + name, n_polys_in_changing_phase) { index_to_node_value_info_ = GetPhaseBasedEEParameterization(); SetNumberOfVariables(index_to_node_value_info_.size()); } NodesVariablesEEForce::OptIndexMap -NodesVariablesEEMotion::GetPhaseBasedEEParameterization () +NodesVariablesEEMotion::GetPhaseBasedEEParameterization() { OptIndexMap index_map; - int idx = 0; // index in variables set - for (int node_id=0; node_id all node values simply set to zero. nodes_.at(id).at(kPos).setZero(); - nodes_.at(id+1).at(kPos).setZero(); + nodes_.at(id + 1).at(kPos).setZero(); nodes_.at(id).at(kVel).setZero(); - nodes_.at(id+1).at(kVel).setZero(); + nodes_.at(id + 1).at(kVel).setZero(); - id += 1; // already added next constant node, so skip + id += 1; // already added next constant node, so skip } } diff --git a/towr/src/parameters.cc b/towr/src/parameters.cc index 2848b58fe..82447ed07 100644 --- a/towr/src/parameters.cc +++ b/towr/src/parameters.cc @@ -30,43 +30,50 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include // fabs #include -#include // std::accumulate -#include // fabs #include +#include // std::accumulate namespace towr { -Parameters::Parameters () +Parameters::Parameters() { // constructs optimization variables - duration_base_polynomial_ = 0.1; + duration_base_polynomial_ = 0.1; force_polynomials_per_stance_phase_ = 3; - ee_polynomials_per_swing_phase_ = 2; // so step can at least lift leg + ee_polynomials_per_swing_phase_ = 2; // so step can at least lift leg // parameters related to specific constraints (only used when it is added as well) force_limit_in_normal_direction_ = 1000; - dt_constraint_range_of_motion_ = 0.08; - dt_constraint_dynamic_ = 0.1; - dt_constraint_base_motion_ = duration_base_polynomial_/4.; // only for base RoM constraint - bound_phase_duration_ = std::make_pair(0.2, 1.0); // used only when optimizing phase durations, so gait + dt_constraint_range_of_motion_ = 0.08; + dt_constraint_dynamic_ = 0.1; + // only for base RoM constraint + dt_constraint_base_motion_ = duration_base_polynomial_ / 4.; + // used only when optimizing phase durations, so gait + bound_phase_duration_ = std::make_pair(0.2, 1.0); // a minimal set of basic constraints constraints_.push_back(Terrain); - constraints_.push_back(Dynamic); //Ensures that the dynamic model is fullfilled at discrete times. - constraints_.push_back(BaseAcc); // so accelerations don't jump between polynomials - constraints_.push_back(EndeffectorRom); //Ensures that the range of motion is respected at discrete times. - constraints_.push_back(Force); // ensures unilateral forces and inside the friction cone. - constraints_.push_back(Swing); // creates smoother swing motions, not absolutely required. + //Ensures that the dynamic model is fullfilled at discrete times. + constraints_.push_back(Dynamic); + // so accelerations don't jump between polynomials + constraints_.push_back(BaseAcc); + //Ensures that the range of motion is respected at discrete times. + constraints_.push_back(EndeffectorRom); + // ensures unilateral forces and inside the friction cone. + constraints_.push_back(Force); + // creates smoother swing motions, not absolutely required. + constraints_.push_back(Swing); // optional costs to e.g penalize endeffector forces // costs_.push_back({ForcesCostID, 1.0}); weighed by 1.0 relative to other costs // bounds on final 6DoF base state - bounds_final_lin_pos_ = {X,Y}; - bounds_final_lin_vel_ = {X,Y,Z}; - bounds_final_ang_pos_ = {X,Y,Z}; - bounds_final_ang_vel_ = {X,Y,Z}; + bounds_final_lin_pos_ = {X, Y}; + bounds_final_lin_vel_ = {X, Y, Z}; + bounds_final_ang_pos_ = {X, Y, Z}; + bounds_final_ang_vel_ = {X, Y, Z}; // additional restrictions are set directly on the variables in nlp_factory, // such as e.g. initial and endeffector,... @@ -79,16 +86,15 @@ Parameters::OptimizePhaseDurations () constraints_.push_back(TotalTime); } -Parameters::VecTimes -Parameters::GetBasePolyDurations () const +Parameters::VecTimes Parameters::GetBasePolyDurations() const { std::vector base_spline_timings_; - double dt = duration_base_polynomial_; - double t_left = GetTotalTime (); + double dt = duration_base_polynomial_; + double t_left = GetTotalTime(); - double eps = 1e-10; // since repeated subtraction causes inaccuracies + double eps = 1e-10; // since repeated subtraction causes inaccuracies while (t_left > eps) { - double duration = t_left>dt? dt : t_left; + double duration = t_left > dt ? dt : t_left; base_spline_timings_.push_back(duration); t_left -= dt; @@ -97,20 +103,17 @@ Parameters::GetBasePolyDurations () const return base_spline_timings_; } -int -Parameters::GetPhaseCount(EEID ee) const +int Parameters::GetPhaseCount(EEID ee) const { return ee_phase_durations_.at(ee).size(); } -int -Parameters::GetEECount() const +int Parameters::GetEECount() const { return ee_in_contact_at_start_.size(); } -double -Parameters::GetTotalTime () const +double Parameters::GetTotalTime() const { std::vector T_feet; @@ -118,20 +121,20 @@ Parameters::GetTotalTime () const T_feet.push_back(std::accumulate(v.begin(), v.end(), 0.0)); // safety check that all feet durations sum to same value - double T = T_feet.empty()? 0.0 : T_feet.front(); // take first foot as reference + double T = + T_feet.empty() ? 0.0 : T_feet.front(); // take first foot as reference for (double Tf : T_feet) assert(fabs(Tf - T) < 1e-6); return T; } -bool -Parameters::IsOptimizeTimings () const +bool Parameters::IsOptimizeTimings() const { // if total time is constrained, then timings are optimized ConstraintName c = TotalTime; - auto v = constraints_; // shorthand + auto v = constraints_; // shorthand return std::find(v.begin(), v.end(), c) != v.end(); } -} // namespace towr +} // namespace towr diff --git a/towr/src/phase_durations.cc b/towr/src/phase_durations.cc index 3a2470849..f366c40ee 100644 --- a/towr/src/phase_durations.cc +++ b/towr/src/phase_durations.cc @@ -29,55 +29,47 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include // std::accumulate +#include // std::accumulate +#include // for Spline::GetSegmentID() #include -#include // for Spline::GetSegmentID() - namespace towr { - -PhaseDurations::PhaseDurations (EndeffectorID ee, - const VecDurations& timings, - bool is_first_phase_in_contact, - double min_duration, - double max_duration) +PhaseDurations::PhaseDurations(EndeffectorID ee, const VecDurations& timings, + bool is_first_phase_in_contact, + double min_duration, double max_duration) // -1 since last phase-duration is not optimized over, but comes from total time - :VariableSet(timings.size()-1, id::EESchedule(ee)) + : VariableSet(timings.size() - 1, id::EESchedule(ee)) { - durations_ = timings; - t_total_ = std::accumulate(timings.begin(), timings.end(), 0.0); + durations_ = timings; + t_total_ = std::accumulate(timings.begin(), timings.end(), 0.0); phase_duration_bounds_ = ifopt::Bounds(min_duration, max_duration); initial_contact_state_ = is_first_phase_in_contact; } -void -PhaseDurations::AddObserver (PhaseDurationsObserver* const o) +void PhaseDurations::AddObserver(PhaseDurationsObserver* const o) { observers_.push_back(o); } -void -PhaseDurations::UpdateObservers () const +void PhaseDurations::UpdateObservers() const { for (auto& spline : observers_) spline->UpdatePolynomialDurations(); } -Eigen::VectorXd -PhaseDurations::GetValues () const +Eigen::VectorXd PhaseDurations::GetValues() const { VectorXd x(GetRows()); - for (int i=0; ix.sum()); + assert(t_total_ > x.sum()); - for (int i=0; iAddObserver(this); } -} // namespace towr - +} // namespace towr diff --git a/towr/src/phase_spline.cc b/towr/src/phase_spline.cc index 2b91bac3d..7b5616f70 100644 --- a/towr/src/phase_spline.cc +++ b/towr/src/phase_spline.cc @@ -27,15 +27,15 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include #include +#include namespace towr { -PhaseSpline::PhaseSpline( - NodesVariablesPhaseBased::Ptr const nodes, - PhaseDurations* const phase_durations) - : NodeSpline(nodes.get(), nodes->ConvertPhaseToPolyDurations(phase_durations->GetPhaseDurations())), +PhaseSpline::PhaseSpline(NodesVariablesPhaseBased::Ptr const nodes, + PhaseDurations* const phase_durations) + : NodeSpline(nodes.get(), nodes->ConvertPhaseToPolyDurations( + phase_durations->GetPhaseDurations())), PhaseDurationsObserver(phase_durations) { phase_nodes_ = nodes; @@ -47,50 +47,54 @@ PhaseSpline::PhaseSpline( // and must make sure that Jacobian structure never changes during // the iterations. // assume every global time time can fall into every polynomial - for (int i=0; iGetPolynomialCount(); ++i) + for (int i = 0; i < nodes->GetPolynomialCount(); ++i) FillJacobianWrtNodes(i, 0.0, kPos, jac_wrt_nodes_structure_, true); } -void -PhaseSpline::UpdatePolynomialDurations() +void PhaseSpline::UpdatePolynomialDurations() { auto phase_duration = phase_durations_->GetPhaseDurations(); - auto poly_durations = phase_nodes_->ConvertPhaseToPolyDurations(phase_duration); + auto poly_durations = + phase_nodes_->ConvertPhaseToPolyDurations(phase_duration); - for (int i=0; iGetPhaseDurations()); + VectorXd dx_dT = GetDerivativeOfPosWrtPhaseDuration(t_global); + VectorXd xd = GetPoint(t_global).v(); + int current_phase = + GetSegmentID(t_global, phase_durations_->GetPhaseDurations()); return phase_durations_->GetJacobianOfPos(current_phase, dx_dT, xd); } -Eigen::VectorXd -PhaseSpline::GetDerivativeOfPosWrtPhaseDuration (double t_global) const +Eigen::VectorXd PhaseSpline::GetDerivativeOfPosWrtPhaseDuration( + double t_global) const { - int poly_id; double t_local; + int poly_id; + double t_local; std::tie(poly_id, t_local) = GetLocalTime(t_global, GetPolyDurations()); - VectorXd vel = GetPoint(t_global).v(); - VectorXd dxdT = cubic_polys_.at(poly_id).GetDerivativeOfPosWrtDuration(t_local); + VectorXd vel = GetPoint(t_global).v(); + VectorXd dxdT = + cubic_polys_.at(poly_id).GetDerivativeOfPosWrtDuration(t_local); - double inner_derivative = phase_nodes_->GetDerivativeOfPolyDurationWrtPhaseDuration(poly_id); - double prev_polys_in_phase = phase_nodes_->GetNumberOfPrevPolynomialsInPhase(poly_id); + double inner_derivative = + phase_nodes_->GetDerivativeOfPolyDurationWrtPhaseDuration(poly_id); + double prev_polys_in_phase = + phase_nodes_->GetNumberOfPrevPolynomialsInPhase(poly_id); // where this minus term comes from: // from number of polynomials before current polynomial that // cause shifting of entire spline - return inner_derivative*(dxdT - prev_polys_in_phase*vel); + return inner_derivative * (dxdT - prev_polys_in_phase * vel); } - } /* namespace towr */ diff --git a/towr/src/polynomial.cc b/towr/src/polynomial.cc index d4382b8bf..f4bbe1b50 100644 --- a/towr/src/polynomial.cc +++ b/towr/src/polynomial.cc @@ -32,13 +32,12 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include - namespace towr { -Polynomial::Polynomial (int order, int dim) +Polynomial::Polynomial(int order, int dim) { - int n_coeff = order+1; - for (int c=A; c(c)); coeff_.push_back(VectorXd::Zero(dim)); } @@ -48,65 +47,67 @@ State Polynomial::GetPoint(double t_local) const { // sanity checks if (t_local < 0.0) - assert(false);//("spliner.cc called with dt<0") + assert(false); //("spliner.cc called with dt<0") int n_dim = coeff_.front().size(); State out(n_dim, 3); for (auto d : {kPos, kVel, kAcc}) for (Coefficients c : coeff_ids_) - out.at(d) += GetDerivativeWrtCoeff(t_local, d, c)*coeff_.at(c); + out.at(d) += GetDerivativeWrtCoeff(t_local, d, c) * coeff_.at(c); return out; } -double -Polynomial::GetDerivativeWrtCoeff (double t, Dx deriv, Coefficients c) const +double Polynomial::GetDerivativeWrtCoeff(double t, Dx deriv, + Coefficients c) const { switch (deriv) { - case kPos: return std::pow(t,c); break; - case kVel: return c>=B? c* std::pow(t,c-1) : 0.0; break; - case kAcc: return c>=C? c*(c-1)*std::pow(t,c-2) : 0.0; break; - default: assert(false); // derivative not defined + case kPos: + return std::pow(t, c); + break; + case kVel: + return c >= B ? c * std::pow(t, c - 1) : 0.0; + break; + case kAcc: + return c >= C ? c * (c - 1) * std::pow(t, c - 2) : 0.0; + break; + default: + assert(false); // derivative not defined } } - - -CubicHermitePolynomial::CubicHermitePolynomial (int dim) - : Polynomial(3,dim), - n0_(dim), - n1_(dim) +CubicHermitePolynomial::CubicHermitePolynomial(int dim) + : Polynomial(3, dim), n0_(dim), n1_(dim) { T_ = 0.0; } -void -CubicHermitePolynomial::SetNodes (const Node& n0, const Node& n1) +void CubicHermitePolynomial::SetNodes(const Node& n0, const Node& n1) { n0_ = n0; n1_ = n1; } -void -CubicHermitePolynomial::SetDuration(double duration) +void CubicHermitePolynomial::SetDuration(double duration) { T_ = duration; } -void -CubicHermitePolynomial::UpdateCoeff() +void CubicHermitePolynomial::UpdateCoeff() { - coeff_[A] = n0_.p(); - coeff_[B] = n0_.v(); - coeff_[C] = -( 3*(n0_.p() - n1_.p()) + T_*(2*n0_.v() + n1_.v()) ) / std::pow(T_,2); - coeff_[D] = ( 2*(n0_.p() - n1_.p()) + T_*( n0_.v() + n1_.v()) ) / std::pow(T_,3); + coeff_[A] = n0_.p(); + coeff_[B] = n0_.v(); + coeff_[C] = -(3 * (n0_.p() - n1_.p()) + T_ * (2 * n0_.v() + n1_.v())) / + std::pow(T_, 2); + coeff_[D] = + (2 * (n0_.p() - n1_.p()) + T_ * (n0_.v() + n1_.v())) / std::pow(T_, 3); } -double -CubicHermitePolynomial::GetDerivativeWrtStartNode (Dx dfdt, - Dx node_derivative, // pos or velocity node - double t_local) const +double CubicHermitePolynomial::GetDerivativeWrtStartNode( + Dx dfdt, + Dx node_derivative, // pos or velocity node + double t_local) const { switch (dfdt) { case kPos: @@ -116,14 +117,14 @@ CubicHermitePolynomial::GetDerivativeWrtStartNode (Dx dfdt, case kAcc: return GetDerivativeOfAccWrtStartNode(node_derivative, t_local); default: - assert(false); // derivative not yet implemented + assert(false); // derivative not yet implemented } } -double -CubicHermitePolynomial::GetDerivativeWrtEndNode (Dx dfdt, - Dx node_derivative, // pos or velocity node - double t_local) const +double CubicHermitePolynomial::GetDerivativeWrtEndNode( + Dx dfdt, + Dx node_derivative, // pos or velocity node + double t_local) const { switch (dfdt) { case kPos: @@ -133,127 +134,138 @@ CubicHermitePolynomial::GetDerivativeWrtEndNode (Dx dfdt, case kAcc: return GetDerivativeOfAccWrtEndNode(node_derivative, t_local); default: - assert(false); // derivative not yet implemented + assert(false); // derivative not yet implemented } } -double -CubicHermitePolynomial::GetDerivativeOfPosWrtStartNode(Dx node_value, - double t) const +double CubicHermitePolynomial::GetDerivativeOfPosWrtStartNode(Dx node_value, + double t) const { - double t2 = std::pow(t,2); - double t3 = std::pow(t,3); + double t2 = std::pow(t, 2); + double t3 = std::pow(t, 3); double T = T_; - double T2 = std::pow(T_,2); - double T3 = std::pow(T_,3); + double T2 = std::pow(T_, 2); + double T3 = std::pow(T_, 3); switch (node_value) { - case kPos: return (2*t3)/T3 - (3*t2)/T2 + 1; - case kVel: return t - (2*t2)/T + t3/T2; - default: assert(false); // only derivative wrt nodes values calculated + case kPos: + return (2 * t3) / T3 - (3 * t2) / T2 + 1; + case kVel: + return t - (2 * t2) / T + t3 / T2; + default: + assert(false); // only derivative wrt nodes values calculated } } -double -CubicHermitePolynomial::GetDerivativeOfVelWrtStartNode (Dx node_value, - double t) const +double CubicHermitePolynomial::GetDerivativeOfVelWrtStartNode(Dx node_value, + double t) const { - double t2 = std::pow(t,2); + double t2 = std::pow(t, 2); double T = T_; - double T2 = std::pow(T_,2); - double T3 = std::pow(T_,3); + double T2 = std::pow(T_, 2); + double T3 = std::pow(T_, 3); switch (node_value) { - case kPos: return (6*t2)/T3 - (6*t)/T2; - case kVel: return (3*t2)/T2 - (4*t)/T + 1; - default: assert(false); // only derivative wrt nodes values calculated + case kPos: + return (6 * t2) / T3 - (6 * t) / T2; + case kVel: + return (3 * t2) / T2 - (4 * t) / T + 1; + default: + assert(false); // only derivative wrt nodes values calculated } } -double -CubicHermitePolynomial::GetDerivativeOfAccWrtStartNode (Dx node_value, - double t) const +double CubicHermitePolynomial::GetDerivativeOfAccWrtStartNode(Dx node_value, + double t) const { double T = T_; - double T2 = std::pow(T_,2); - double T3 = std::pow(T_,3); + double T2 = std::pow(T_, 2); + double T3 = std::pow(T_, 3); switch (node_value) { - case kPos: return (12*t)/T3 - 6/T2; - case kVel: return (6*t)/T2 - 4/T; - default: assert(false); // only derivative wrt nodes values calculated + case kPos: + return (12 * t) / T3 - 6 / T2; + case kVel: + return (6 * t) / T2 - 4 / T; + default: + assert(false); // only derivative wrt nodes values calculated } } -double -CubicHermitePolynomial::GetDerivativeOfPosWrtEndNode (Dx node_value, - double t) const +double CubicHermitePolynomial::GetDerivativeOfPosWrtEndNode(Dx node_value, + double t) const { - double t2 = std::pow(t,2); - double t3 = std::pow(t,3); + double t2 = std::pow(t, 2); + double t3 = std::pow(t, 3); double T = T_; - double T2 = std::pow(T_,2); - double T3 = std::pow(T_,3); + double T2 = std::pow(T_, 2); + double T3 = std::pow(T_, 3); switch (node_value) { - case kPos: return (3*t2)/T2 - (2*t3)/T3; - case kVel: return t3/T2 - t2/T; - default: assert(false); // only derivative wrt nodes values calculated + case kPos: + return (3 * t2) / T2 - (2 * t3) / T3; + case kVel: + return t3 / T2 - t2 / T; + default: + assert(false); // only derivative wrt nodes values calculated } } -double -CubicHermitePolynomial::GetDerivativeOfVelWrtEndNode (Dx node_value, - double t) const +double CubicHermitePolynomial::GetDerivativeOfVelWrtEndNode(Dx node_value, + double t) const { - double t2 = std::pow(t,2); + double t2 = std::pow(t, 2); double T = T_; - double T2 = std::pow(T_,2); - double T3 = std::pow(T_,3); + double T2 = std::pow(T_, 2); + double T3 = std::pow(T_, 3); switch (node_value) { - case kPos: return (6*t)/T2 - (6*t2)/T3; - case kVel: return (3*t2)/T2 - (2*t)/T; - default: assert(false); // only derivative wrt nodes values calculated + case kPos: + return (6 * t) / T2 - (6 * t2) / T3; + case kVel: + return (3 * t2) / T2 - (2 * t) / T; + default: + assert(false); // only derivative wrt nodes values calculated } } -double -CubicHermitePolynomial::GetDerivativeOfAccWrtEndNode (Dx node_value, - double t) const +double CubicHermitePolynomial::GetDerivativeOfAccWrtEndNode(Dx node_value, + double t) const { double T = T_; - double T2 = std::pow(T_,2); - double T3 = std::pow(T_,3); + double T2 = std::pow(T_, 2); + double T3 = std::pow(T_, 3); switch (node_value) { - case kPos: return 6/T2 - (12*t)/T3; - case kVel: return (6*t)/T2 - 2/T; - default: assert(false); // only derivative wrt nodes values calculated + case kPos: + return 6 / T2 - (12 * t) / T3; + case kVel: + return (6 * t) / T2 - 2 / T; + default: + assert(false); // only derivative wrt nodes values calculated } } -Eigen::VectorXd -CubicHermitePolynomial::GetDerivativeOfPosWrtDuration(double t) const +Eigen::VectorXd CubicHermitePolynomial::GetDerivativeOfPosWrtDuration( + double t) const { VectorXd x0 = n0_.p(); VectorXd x1 = n1_.p(); VectorXd v0 = n0_.v(); VectorXd v1 = n1_.v(); - double t2 = std::pow(t,2); - double t3 = std::pow(t,3); + double t2 = std::pow(t, 2); + double t3 = std::pow(t, 3); double T = T_; - double T2 = std::pow(T_,2); - double T3 = std::pow(T_,3); - double T4 = std::pow(T_,4); + double T2 = std::pow(T_, 2); + double T3 = std::pow(T_, 3); + double T4 = std::pow(T_, 4); - VectorXd deriv = (t3*(v0 + v1))/T3 - - (t2*(2*v0 + v1))/T2 - - (3*t3*(2*x0 - 2*x1 + T*v0 + T*v1))/T4 - + (2*t2*(3*x0 - 3*x1 + 2*T*v0 + T*v1))/T3; + VectorXd deriv = (t3 * (v0 + v1)) / T3 - (t2 * (2 * v0 + v1)) / T2 - + (3 * t3 * (2 * x0 - 2 * x1 + T * v0 + T * v1)) / T4 + + (2 * t2 * (3 * x0 - 3 * x1 + 2 * T * v0 + T * v1)) / T3; return deriv; } -} // namespace towr +} // namespace towr diff --git a/towr/src/quadruped_gait_generator.cc b/towr/src/quadruped_gait_generator.cc index ceee50581..3a9643402 100644 --- a/towr/src/quadruped_gait_generator.cc +++ b/towr/src/quadruped_gait_generator.cc @@ -36,16 +36,18 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { -QuadrupedGaitGenerator::QuadrupedGaitGenerator () +QuadrupedGaitGenerator::QuadrupedGaitGenerator() { int n_ee = 4; ContactState init(n_ee, false); + // clang-format off II_ = init; // flight_phase PI_ = bI_ = IP_ = Ib_ = init; // single contact Pb_ = bP_ = BI_ = IB_ = PP_ = bb_ = init; // two leg support Bb_ = BP_ = bB_ = PB_ = init; // three-leg support BB_ = init; // four-leg support phase + // clang-format on // flight_phase II_ = ContactState(n_ee, false); @@ -55,17 +57,31 @@ QuadrupedGaitGenerator::QuadrupedGaitGenerator () IP_.at(LF) = true; Ib_.at(RF) = true; // two stancelegs - Pb_.at(LH) = true; Pb_.at(RF) = true; - bP_.at(RH) = true; bP_.at(LF) = true; - BI_.at(LH) = true; BI_.at(RH) = true; - IB_.at(LF) = true; IB_.at(RF) = true; - PP_.at(LH) = true; PP_.at(LF) = true; - bb_.at(RH) = true; bb_.at(RF) = true; + Pb_.at(LH) = true; + Pb_.at(RF) = true; + bP_.at(RH) = true; + bP_.at(LF) = true; + BI_.at(LH) = true; + BI_.at(RH) = true; + IB_.at(LF) = true; + IB_.at(RF) = true; + PP_.at(LH) = true; + PP_.at(LF) = true; + bb_.at(RH) = true; + bb_.at(RF) = true; // three stancelegs - Bb_.at(LH) = true; Bb_.at(RH) = true; Bb_.at(RF)= true; - BP_.at(LH) = true; BP_.at(RH) = true; BP_.at(LF)= true; - bB_.at(RH) = true; bB_.at(LF) = true; bB_.at(RF)= true; - PB_.at(LH) = true; PB_.at(LF) = true; PB_.at(RF)= true; + Bb_.at(LH) = true; + Bb_.at(RH) = true; + Bb_.at(RF) = true; + BP_.at(LH) = true; + BP_.at(RH) = true; + BP_.at(LF) = true; + bB_.at(RH) = true; + bB_.at(LF) = true; + bB_.at(RF) = true; + PB_.at(LH) = true; + PB_.at(LF) = true; + PB_.at(RF) = true; // four stancelgs BB_ = ContactState(n_ee, true); @@ -73,293 +89,274 @@ QuadrupedGaitGenerator::QuadrupedGaitGenerator () SetGaits({Stand}); } -void -QuadrupedGaitGenerator::SetCombo (Combos combo) +void QuadrupedGaitGenerator::SetCombo(Combos combo) { switch (combo) { - case C0: SetGaits({Stand, Walk2, Walk2, Walk2, Walk2E, Stand}); break; // overlap-walk - case C1: SetGaits({Stand, Run2, Run2, Run2, Run2E, Stand}); break; // fly trot - case C2: SetGaits({Stand, Run3, Run3, Run3, Run3E, Stand}); break; // pace - case C3: SetGaits({Stand, Hop1, Hop1, Hop1, Hop1E, Stand}); break; // bound - case C4: SetGaits({Stand, Hop3, Hop3, Hop3, Hop3E, Stand}); break; // gallop - default: assert(false); std::cout << "Gait not defined\n"; break; + case C0: + SetGaits({Stand, Walk2, Walk2, Walk2, Walk2E, Stand}); + break; // overlap-walk + case C1: + SetGaits({Stand, Run2, Run2, Run2, Run2E, Stand}); + break; // fly trot + case C2: + SetGaits({Stand, Run3, Run3, Run3, Run3E, Stand}); + break; // pace + case C3: + SetGaits({Stand, Hop1, Hop1, Hop1, Hop1E, Stand}); + break; // bound + case C4: + SetGaits({Stand, Hop3, Hop3, Hop3, Hop3E, Stand}); + break; // gallop + default: + assert(false); + std::cout << "Gait not defined\n"; + break; } } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetGait(Gaits gait) const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetGait( + Gaits gait) const { switch (gait) { - case Stand: return GetStrideStand(); - case Flight: return GetStrideFlight(); - case Walk1: return GetStrideWalk(); - case Walk2: return GetStrideWalkOverlap(); - case Walk2E: return RemoveTransition(GetStrideWalkOverlap()); - case Run1: return GetStrideTrot(); - case Run2: return GetStrideTrotFly(); - case Run2E: return GetStrideTrotFlyEnd(); - case Run3: return GetStridePace(); - case Run3E: return GetStridePaceEnd(); - case Hop1: return GetStrideBound(); - case Hop1E: return GetStrideBoundEnd(); - case Hop2: return GetStridePronk(); - case Hop3: return GetStrideGallop(); - case Hop3E: return RemoveTransition(GetStrideGallop()); - case Hop5: return GetStrideLimp(); - default: assert(false); // gait not implemented + case Stand: + return GetStrideStand(); + case Flight: + return GetStrideFlight(); + case Walk1: + return GetStrideWalk(); + case Walk2: + return GetStrideWalkOverlap(); + case Walk2E: + return RemoveTransition(GetStrideWalkOverlap()); + case Run1: + return GetStrideTrot(); + case Run2: + return GetStrideTrotFly(); + case Run2E: + return GetStrideTrotFlyEnd(); + case Run3: + return GetStridePace(); + case Run3E: + return GetStridePaceEnd(); + case Hop1: + return GetStrideBound(); + case Hop1E: + return GetStrideBoundEnd(); + case Hop2: + return GetStridePronk(); + case Hop3: + return GetStrideGallop(); + case Hop3E: + return RemoveTransition(GetStrideGallop()); + case Hop5: + return GetStrideLimp(); + default: + assert(false); // gait not implemented } } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStrideStand () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStrideStand() const { - auto times = - { + auto times = { 0.3, }; - auto contacts = - { + auto contacts = { BB_, }; return std::make_pair(times, contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStrideFlight () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStrideFlight() const { - auto times = - { + auto times = { 0.3, }; - auto contacts = - { + auto contacts = { Bb_, }; return std::make_pair(times, contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStridePronk () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStridePronk() const { double push = 0.3; double flight = 0.4; double land = 0.3; - auto times = - { - push, flight, land - }; - auto phase_contacts = - { - BB_, II_, BB_, + auto times = {push, flight, land}; + auto phase_contacts = { + BB_, + II_, + BB_, }; return std::make_pair(times, phase_contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStrideWalk () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStrideWalk() const { double step = 0.3; double stand = 0.2; - auto times = - { - step, stand, step, stand, - step, stand, step, stand, - }; - auto phase_contacts = - { - bB_, BB_, Bb_, BB_, - PB_, BB_, BP_, BB_ + auto times = { + step, stand, step, stand, step, stand, step, stand, }; + auto phase_contacts = {bB_, BB_, Bb_, BB_, PB_, BB_, BP_, BB_}; return std::make_pair(times, phase_contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStrideWalkOverlap () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStrideWalkOverlap() + const { double three = 0.25; double lateral = 0.13; double diagonal = 0.13; - auto times = - { - three, lateral, three, - diagonal, - three, lateral, three, - diagonal, + auto times = { + three, lateral, three, diagonal, three, lateral, three, diagonal, }; - auto phase_contacts = - { + auto phase_contacts = { bB_, bb_, Bb_, - Pb_, // start lifting RH + Pb_, // start lifting RH PB_, PP_, BP_, - bP_, // start lifting LH + bP_, // start lifting LH }; return std::make_pair(times, phase_contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStrideTrot () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStrideTrot() const { - double t_step = 0.3; + double t_step = 0.3; double t_stand = 0.2; - auto times = - { - t_step, t_stand, t_step, t_stand, + auto times = { + t_step, + t_stand, + t_step, + t_stand, }; - auto phase_contacts = - { - bP_, BB_, Pb_, BB_, + auto phase_contacts = { + bP_, + BB_, + Pb_, + BB_, }; return std::make_pair(times, phase_contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStrideTrotFly () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStrideTrotFly() + const { - double stand = 0.4; + double stand = 0.4; double flight = 0.1; - auto times = - { - stand, flight, - stand, flight, + auto times = { + stand, + flight, + stand, + flight, }; - auto phase_contacts = - { - bP_, II_, - Pb_, II_, + auto phase_contacts = { + bP_, + II_, + Pb_, + II_, }; return std::make_pair(times, phase_contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStrideTrotFlyEnd () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStrideTrotFlyEnd() + const { - auto times = - { + auto times = { 0.4, }; - auto phase_contacts = - { - bP_ - }; + auto phase_contacts = {bP_}; return std::make_pair(times, phase_contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStridePace () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStridePace() const { double stand = 0.3; double flight = 0.1; - auto times = - { - stand, flight, stand, flight - }; - auto phase_contacts = - { - PP_, II_, bb_, II_, + auto times = {stand, flight, stand, flight}; + auto phase_contacts = { + PP_, + II_, + bb_, + II_, }; return std::make_pair(times, phase_contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStridePaceEnd () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStridePaceEnd() + const { - auto times = - { + auto times = { 0.3, }; - auto phase_contacts = - { + auto phase_contacts = { PP_, }; return std::make_pair(times, phase_contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStrideBound () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStrideBound() const { double stand = 0.3; double flight = 0.1; - auto times = - { - stand, flight, stand, flight - }; - auto phase_contacts = - { - BI_, II_, IB_, II_ - }; + auto times = {stand, flight, stand, flight}; + auto phase_contacts = {BI_, II_, IB_, II_}; return std::make_pair(times, phase_contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStrideBoundEnd () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStrideBoundEnd() + const { - auto times = - { + auto times = { 0.3, }; - auto phase_contacts = - { + auto phase_contacts = { BI_, }; return std::make_pair(times, phase_contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStrideGallop () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStrideGallop() const { - double A = 0.3; // both feet in air - double B = 0.2; // overlap - double C = 0.2; // transition front->hind - auto times = - { - B, A, B, - C, - B, A, B, - C - }; - auto phase_contacts = - { - Bb_, BI_, BP_, // front legs swing forward - bP_, // transition phase - bB_, IB_, PB_, // hind legs swing forward - Pb_ - }; + double A = 0.3; // both feet in air + double B = 0.2; // overlap + double C = 0.2; // transition front->hind + auto times = {B, A, B, C, B, A, B, C}; + auto phase_contacts = {Bb_, BI_, BP_, // front legs swing forward + bP_, // transition phase + bB_, IB_, PB_, // hind legs swing forward + Pb_}; return std::make_pair(times, phase_contacts); } -QuadrupedGaitGenerator::GaitInfo -QuadrupedGaitGenerator::GetStrideLimp () const +QuadrupedGaitGenerator::GaitInfo QuadrupedGaitGenerator::GetStrideLimp() const { - double A = 0.1; // three in contact - double B = 0.2; // all in contact - double C = 0.1; // one in contact - - auto times = - { - A, B, C, - A, B, C, + double A = 0.1; // three in contact + double B = 0.2; // all in contact + double C = 0.1; // one in contact + + auto times = { + A, B, C, A, B, C, }; - auto phase_contacts = - { - Bb_, BB_, IP_, - Bb_, BB_, IP_, + auto phase_contacts = { + Bb_, BB_, IP_, Bb_, BB_, IP_, }; return std::make_pair(times, phase_contacts); diff --git a/towr/src/range_of_motion_constraint.cc b/towr/src/range_of_motion_constraint.cc index 5c4f485fc..5d70a6ce1 100644 --- a/towr/src/range_of_motion_constraint.cc +++ b/towr/src/range_of_motion_constraint.cc @@ -32,11 +32,10 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { -RangeOfMotionConstraint::RangeOfMotionConstraint (const KinematicModel::Ptr& model, - double T, double dt, - const EE& ee, - const SplineHolder& spline_holder) - :TimeDiscretizationConstraint(T, dt, "rangeofmotion-" + std::to_string(ee)) +RangeOfMotionConstraint::RangeOfMotionConstraint( + const KinematicModel::Ptr& model, double T, double dt, const EE& ee, + const SplineHolder& spline_holder) + : TimeDiscretizationConstraint(T, dt, "rangeofmotion-" + std::to_string(ee)) { base_linear_ = spline_holder.base_linear_; base_angular_ = EulerConverter(spline_holder.base_angular_); @@ -44,69 +43,72 @@ RangeOfMotionConstraint::RangeOfMotionConstraint (const KinematicModel::Ptr& mod max_deviation_from_nominal_ = model->GetMaximumDeviationFromNominal(); nominal_ee_pos_B_ = model->GetNominalStanceInBase().at(ee); - ee_ = ee; + ee_ = ee; - SetRows(GetNumberOfNodes()*k3D); + SetRows(GetNumberOfNodes() * k3D); } -int -RangeOfMotionConstraint::GetRow (int node, int dim) const +int RangeOfMotionConstraint::GetRow(int node, int dim) const { - return node*k3D + dim; + return node * k3D + dim; } -void -RangeOfMotionConstraint::UpdateConstraintAtInstance (double t, int k, VectorXd& g) const +void RangeOfMotionConstraint::UpdateConstraintAtInstance(double t, int k, + VectorXd& g) const { - Vector3d base_W = base_linear_->GetPoint(t).p(); + Vector3d base_W = base_linear_->GetPoint(t).p(); Vector3d pos_ee_W = ee_motion_->GetPoint(t).p(); - EulerConverter::MatrixSXd b_R_w = base_angular_.GetRotationMatrixBaseToWorld(t).transpose(); + EulerConverter::MatrixSXd b_R_w = + base_angular_.GetRotationMatrixBaseToWorld(t).transpose(); Vector3d vector_base_to_ee_W = pos_ee_W - base_W; - Vector3d vector_base_to_ee_B = b_R_w*(vector_base_to_ee_W); + Vector3d vector_base_to_ee_B = b_R_w * (vector_base_to_ee_W); g.middleRows(GetRow(k, X), k3D) = vector_base_to_ee_B; } -void -RangeOfMotionConstraint::UpdateBoundsAtInstance (double t, int k, VecBound& bounds) const +void RangeOfMotionConstraint::UpdateBoundsAtInstance(double t, int k, + VecBound& bounds) const { - for (int dim=0; dimGetJacobianWrtNodes(t, kPos); + jac.middleRows(row_start, k3D) = + -1 * b_R_w * base_linear_->GetJacobianWrtNodes(t, kPos); } if (var_set == id::base_ang_nodes) { Vector3d base_W = base_linear_->GetPoint(t).p(); Vector3d ee_pos_W = ee_motion_->GetPoint(t).p(); - Vector3d r_W = ee_pos_W - base_W; - jac.middleRows(row_start, k3D) = base_angular_.DerivOfRotVecMult(t,r_W, true); + Vector3d r_W = ee_pos_W - base_W; + jac.middleRows(row_start, k3D) = + base_angular_.DerivOfRotVecMult(t, r_W, true); } if (var_set == id::EEMotionNodes(ee_)) { - jac.middleRows(row_start, k3D) = b_R_w*ee_motion_->GetJacobianWrtNodes(t,kPos); + jac.middleRows(row_start, k3D) = + b_R_w * ee_motion_->GetJacobianWrtNodes(t, kPos); } if (var_set == id::EESchedule(ee_)) { - jac.middleRows(row_start, k3D) = b_R_w*ee_motion_->GetJacobianOfPosWrtDurations(t); + jac.middleRows(row_start, k3D) = + b_R_w * ee_motion_->GetJacobianOfPosWrtDurations(t); } } -} /* namespace xpp */ - +} // namespace towr diff --git a/towr/src/robot_model.cc b/towr/src/robot_model.cc index e349b1296..54fe5b892 100644 --- a/towr/src/robot_model.cc +++ b/towr/src/robot_model.cc @@ -29,14 +29,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include +#include #include #include -#include +#include namespace towr { - RobotModel::RobotModel(Robot robot) { switch (robot) { @@ -57,12 +56,9 @@ RobotModel::RobotModel(Robot robot) kinematic_model_ = std::make_shared(); break; default: - assert(false); // Error: Robot model not implemented. + assert(false); // Error: Robot model not implemented. break; } } - -} // namespace towr - - +} // namespace towr diff --git a/towr/src/single_rigid_body_dynamics.cc b/towr/src/single_rigid_body_dynamics.cc index 5fdc565b0..d5c0741e0 100644 --- a/towr/src/single_rigid_body_dynamics.cc +++ b/towr/src/single_rigid_body_dynamics.cc @@ -33,128 +33,124 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { // some Eigen helper functions -static Eigen::Matrix3d BuildInertiaTensor( double Ixx, double Iyy, double Izz, - double Ixy, double Ixz, double Iyz) +static Eigen::Matrix3d BuildInertiaTensor(double Ixx, double Iyy, double Izz, + double Ixy, double Ixz, double Iyz) { Eigen::Matrix3d I; - I << Ixx, -Ixy, -Ixz, - -Ixy, Iyy, -Iyz, - -Ixz, -Iyz, Izz; + I << Ixx, -Ixy, -Ixz, -Ixy, Iyy, -Iyz, -Ixz, -Iyz, Izz; return I; } // builds a cross product matrix out of "in", so in x v = X(in)*v -SingleRigidBodyDynamics::Jac -Cross(const Eigen::Vector3d& in) +SingleRigidBodyDynamics::Jac Cross(const Eigen::Vector3d& in) { - SingleRigidBodyDynamics::Jac out(3,3); + SingleRigidBodyDynamics::Jac out(3, 3); - out.coeffRef(0,1) = -in(2); out.coeffRef(0,2) = in(1); - out.coeffRef(1,0) = in(2); out.coeffRef(1,2) = -in(0); - out.coeffRef(2,0) = -in(1); out.coeffRef(2,1) = in(0); + out.coeffRef(0, 1) = -in(2); + out.coeffRef(0, 2) = in(1); + out.coeffRef(1, 0) = in(2); + out.coeffRef(1, 2) = -in(0); + out.coeffRef(2, 0) = -in(1); + out.coeffRef(2, 1) = in(0); return out; } -SingleRigidBodyDynamics::SingleRigidBodyDynamics (double mass, - double Ixx, double Iyy, double Izz, - double Ixy, double Ixz, double Iyz, - int ee_count) - : SingleRigidBodyDynamics(mass, - BuildInertiaTensor(Ixx, Iyy, Izz, Ixy, Ixz, Iyz), - ee_count) -{ -} - -SingleRigidBodyDynamics::SingleRigidBodyDynamics (double mass, const Eigen::Matrix3d& inertia_b, - int ee_count) - :DynamicModel(mass, ee_count) +SingleRigidBodyDynamics::SingleRigidBodyDynamics(double mass, double Ixx, + double Iyy, double Izz, + double Ixy, double Ixz, + double Iyz, int ee_count) + : SingleRigidBodyDynamics( + mass, BuildInertiaTensor(Ixx, Iyy, Izz, Ixy, Ixz, Iyz), ee_count) +{} + +SingleRigidBodyDynamics::SingleRigidBodyDynamics( + double mass, const Eigen::Matrix3d& inertia_b, int ee_count) + : DynamicModel(mass, ee_count) { I_b = inertia_b.sparseView(); } -SingleRigidBodyDynamics::BaseAcc -SingleRigidBodyDynamics::GetDynamicViolation () const +SingleRigidBodyDynamics::BaseAcc SingleRigidBodyDynamics::GetDynamicViolation() + const { // https://en.wikipedia.org/wiki/Newton%E2%80%93Euler_equations Vector3d f_sum, tau_sum; - f_sum.setZero(); tau_sum.setZero(); + f_sum.setZero(); + tau_sum.setZero(); - for (int ee=0; eeGetName()) +SoftConstraint::SoftConstraint(const ConstraintPtr& constraint) + : Component(1, "soft-" + constraint->GetName()) { - constraint_ = constraint; + constraint_ = constraint; int n_constraints = constraint_->GetRows(); // average value of each upper and lower bound - b_ = VectorXd(n_constraints); - int i=0; + b_ = VectorXd(n_constraints); + int i = 0; for (auto b : constraint_->GetBounds()) { - b_(i++) = (b.upper_ + b.lower_)/2.; + b_(i++) = (b.upper_ + b.lower_) / 2.; } // treat all constraints equally by default @@ -49,20 +49,18 @@ SoftConstraint::SoftConstraint (const ConstraintPtr& constraint) W_.setOnes(); } -SoftConstraint::VectorXd -SoftConstraint::GetValues () const +SoftConstraint::VectorXd SoftConstraint::GetValues() const { - VectorXd g = constraint_->GetValues(); - VectorXd cost = 0.5*(g-b_).transpose()*W_.asDiagonal()*(g-b_); + VectorXd g = constraint_->GetValues(); + VectorXd cost = 0.5 * (g - b_).transpose() * W_.asDiagonal() * (g - b_); return cost; } -SoftConstraint::Jacobian -SoftConstraint::GetJacobian () const +SoftConstraint::Jacobian SoftConstraint::GetJacobian() const { - VectorXd g = constraint_->GetValues(); - Jacobian jac = constraint_->GetJacobian(); - VectorXd grad = jac.transpose()*W_.asDiagonal()*(g-b_); + VectorXd g = constraint_->GetValues(); + Jacobian jac = constraint_->GetJacobian(); + VectorXd grad = jac.transpose() * W_.asDiagonal() * (g - b_); return grad.transpose().sparseView(); } diff --git a/towr/src/spline.cc b/towr/src/spline.cc index 2ff97177f..dafe25a29 100644 --- a/towr/src/spline.cc +++ b/towr/src/spline.cc @@ -29,7 +29,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include // std::accumulate +#include // std::accumulate namespace towr { @@ -38,75 +38,70 @@ Spline::Spline(const VecTimes& poly_durations, int n_dim) uint n_polys = poly_durations.size(); cubic_polys_.assign(n_polys, CubicHermitePolynomial(n_dim)); - for (int i=0; i= 0.0); - double t = 0; - int i=0; - for (double d: durations) { - t += d; + double t = 0; + int i = 0; + for (double d : durations) { + t += d; - if (t >= t_global-eps) // at junctions, returns previous spline (=) - return i; + if (t >= t_global - eps) // at junctions, returns previous spline (=) + return i; - i++; - } + i++; + } - assert(false); // this should never be reached + assert(false); // this should never be reached } -std::pair -Spline::GetLocalTime (double t_global, const VecTimes& durations) const +std::pair Spline::GetLocalTime(double t_global, + const VecTimes& durations) const { int id = GetSegmentID(t_global, durations); double t_local = t_global; - for (int i=0; iGetPoint(0.0).p().rows(); n_junctions_ = spline->GetPolynomialCount() - 1; T_ = spline->GetPolyDurations(); - SetRows(n_dim_*n_junctions_); + SetRows(n_dim_ * n_junctions_); } -Eigen::VectorXd -SplineAccConstraint::GetValues () const +Eigen::VectorXd SplineAccConstraint::GetValues() const { VectorXd g(GetRows()); - for (int j=0; jGetPoint(p_prev, T_.at(p_prev)).a(); - int p_next = j+1; + int p_next = j + 1; VectorXd acc_next = spline_->GetPoint(p_next, 0.0).a(); - g.segment(j*n_dim_, n_dim_) = acc_prev - acc_next; + g.segment(j * n_dim_, n_dim_) = acc_prev - acc_next; } return g; } -void -SplineAccConstraint::FillJacobianBlock (std::string var_set, Jacobian& jac) const +void SplineAccConstraint::FillJacobianBlock(std::string var_set, + Jacobian& jac) const { if (var_set == node_variables_id_) { - for (int j=0; jGetJacobianWrtNodes(p_prev, T_.at(p_prev), kAcc); + for (int j = 0; j < n_junctions_; ++j) { + int p_prev = j; // id of previous polynomial + Jacobian acc_prev = + spline_->GetJacobianWrtNodes(p_prev, T_.at(p_prev), kAcc); - int p_next = j+1; + int p_next = j + 1; Jacobian acc_next = spline_->GetJacobianWrtNodes(p_next, 0.0, kAcc); - jac.middleRows(j*n_dim_, n_dim_) = acc_prev - acc_next; + jac.middleRows(j * n_dim_, n_dim_) = acc_prev - acc_next; } } } -SplineAccConstraint::VecBound -SplineAccConstraint::GetBounds () const +SplineAccConstraint::VecBound SplineAccConstraint::GetBounds() const { return VecBound(GetRows(), ifopt::BoundZero); } -} /* namespace xpp */ - +} // namespace towr diff --git a/towr/src/spline_holder.cc b/towr/src/spline_holder.cc index 5cdf440cf..e8cabfa3c 100644 --- a/towr/src/spline_holder.cc +++ b/towr/src/spline_holder.cc @@ -27,35 +27,44 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include #include +#include -namespace towr{ +namespace towr { -SplineHolder::SplineHolder (NodesVariables::Ptr base_lin_nodes, - NodesVariables::Ptr base_ang_nodes, - const std::vector& base_poly_durations, - std::vector ee_motion_nodes, - std::vector ee_force_nodes, - std::vector phase_durations, - bool durations_change) +SplineHolder::SplineHolder( + NodesVariables::Ptr base_lin_nodes, NodesVariables::Ptr base_ang_nodes, + const std::vector& base_poly_durations, + std::vector ee_motion_nodes, + std::vector ee_force_nodes, + std::vector phase_durations, bool durations_change) { - base_linear_ = std::make_shared(base_lin_nodes.get(), base_poly_durations); - base_angular_ = std::make_shared(base_ang_nodes.get(), base_poly_durations); + base_linear_ = + std::make_shared(base_lin_nodes.get(), base_poly_durations); + base_angular_ = + std::make_shared(base_ang_nodes.get(), base_poly_durations); phase_durations_ = phase_durations; - for (uint ee=0; ee(ee_motion_nodes.at(ee), phase_durations.at(ee).get())); - ee_force_.push_back(std::make_shared(ee_force_nodes.at(ee), phase_durations.at(ee).get())); + ee_motion_.push_back(std::make_shared( + ee_motion_nodes.at(ee), phase_durations.at(ee).get())); + ee_force_.push_back(std::make_shared( + ee_force_nodes.at(ee), phase_durations.at(ee).get())); } else { // spline without changing the polynomial durations - auto ee_motion_poly_durations = ee_motion_nodes.at(ee)->ConvertPhaseToPolyDurations(phase_durations.at(ee)->GetPhaseDurations()); - auto ee_force_poly_durations = ee_force_nodes.at(ee)->ConvertPhaseToPolyDurations(phase_durations.at(ee)->GetPhaseDurations()); + auto ee_motion_poly_durations = + ee_motion_nodes.at(ee)->ConvertPhaseToPolyDurations( + phase_durations.at(ee)->GetPhaseDurations()); + auto ee_force_poly_durations = + ee_force_nodes.at(ee)->ConvertPhaseToPolyDurations( + phase_durations.at(ee)->GetPhaseDurations()); - ee_motion_.push_back(std::make_shared(ee_motion_nodes.at(ee).get(), ee_motion_poly_durations)); - ee_force_.push_back (std::make_shared(ee_force_nodes.at(ee).get(), ee_force_poly_durations)); + ee_motion_.push_back(std::make_shared( + ee_motion_nodes.at(ee).get(), ee_motion_poly_durations)); + ee_force_.push_back(std::make_shared( + ee_force_nodes.at(ee).get(), ee_force_poly_durations)); } } } diff --git a/towr/src/state.cc b/towr/src/state.cc index a299ab612..0545aaad4 100644 --- a/towr/src/state.cc +++ b/towr/src/state.cc @@ -29,43 +29,36 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include - namespace towr { -State::State (int dim, int n_derivatives) +State::State(int dim, int n_derivatives) { values_ = std::vector(n_derivatives, VectorXd::Zero(dim)); } -const Eigen::VectorXd -State::at (Dx deriv) const +const Eigen::VectorXd State::at(Dx deriv) const { return values_.at(deriv); } -Eigen::VectorXd& -State::at (Dx deriv) +Eigen::VectorXd& State::at(Dx deriv) { return values_.at(deriv); } -const Eigen::VectorXd -State::p () const +const Eigen::VectorXd State::p() const { return at(kPos); } -const Eigen::VectorXd -State::v () const +const Eigen::VectorXd State::v() const { return at(kVel); } -const Eigen::VectorXd -State::a () const +const Eigen::VectorXd State::a() const { return at(kAcc); } -} // namespace towr - +} // namespace towr diff --git a/towr/src/swing_constraint.cc b/towr/src/swing_constraint.cc index fdeec7508..c7fb59dab 100644 --- a/towr/src/swing_constraint.cc +++ b/towr/src/swing_constraint.cc @@ -32,43 +32,44 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { -SwingConstraint::SwingConstraint (std::string ee_motion) - :ConstraintSet(kSpecifyLater, "swing-" + ee_motion) +SwingConstraint::SwingConstraint(std::string ee_motion) + : ConstraintSet(kSpecifyLater, "swing-" + ee_motion) { ee_motion_id_ = ee_motion; } -void -towr::SwingConstraint::InitVariableDependedQuantities (const VariablesPtr& x) +void towr::SwingConstraint::InitVariableDependedQuantities( + const VariablesPtr& x) { ee_motion_ = x->GetComponent(ee_motion_id_); pure_swing_node_ids_ = ee_motion_->GetIndicesOfNonConstantNodes(); // constrain xy position and velocity of every swing node - int constraint_count = pure_swing_node_ids_.size()*Node::n_derivatives*k2D; + int constraint_count = + pure_swing_node_ids_.size() * Node::n_derivatives * k2D; SetRows(constraint_count); } -Eigen::VectorXd -SwingConstraint::GetValues () const +Eigen::VectorXd SwingConstraint::GetValues() const { VectorXd g(GetRows()); - int row = 0; + int row = 0; auto nodes = ee_motion_->GetNodes(); for (int node_id : pure_swing_node_ids_) { // assumes two splines per swingphase and starting and ending in stance auto curr = nodes.at(node_id); - Vector2d prev = nodes.at(node_id-1).p().topRows(); - Vector2d next = nodes.at(node_id+1).p().topRows(); + Vector2d prev = nodes.at(node_id - 1).p().topRows(); + Vector2d next = nodes.at(node_id + 1).p().topRows(); - Vector2d distance_xy = next - prev; - Vector2d xy_center = prev + 0.5*distance_xy; - Vector2d des_vel_center = distance_xy/t_swing_avg_; // linear interpolation not accurate - for (auto dim : {X,Y}) { + Vector2d distance_xy = next - prev; + Vector2d xy_center = prev + 0.5 * distance_xy; + Vector2d des_vel_center = + distance_xy / t_swing_avg_; // linear interpolation not accurate + for (auto dim : {X, Y}) { g(row++) = curr.p()(dim) - xy_center(dim); g(row++) = curr.v()(dim) - des_vel_center(dim); } @@ -77,30 +78,37 @@ SwingConstraint::GetValues () const return g; } -SwingConstraint::VecBound -SwingConstraint::GetBounds () const +SwingConstraint::VecBound SwingConstraint::GetBounds() const { return VecBound(GetRows(), ifopt::BoundZero); } -void -SwingConstraint::FillJacobianBlock (std::string var_set, - Jacobian& jac) const +void SwingConstraint::FillJacobianBlock(std::string var_set, + Jacobian& jac) const { if (var_set == ee_motion_->GetName()) { int row = 0; for (int node_id : pure_swing_node_ids_) { - for (auto dim : {X,Y}) { + for (auto dim : {X, Y}) { // position constraint - jac.coeffRef(row, ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(node_id, kPos, dim))) = 1.0; // current node - jac.coeffRef(row, ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(node_id+1, kPos, dim))) = -0.5; // next node - jac.coeffRef(row, ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(node_id-1, kPos, dim))) = -0.5; // previous node + jac.coeffRef(row, ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo( + node_id, kPos, dim))) = 1.0; // current node + jac.coeffRef(row, ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo( + node_id + 1, kPos, dim))) = -0.5; // next node + jac.coeffRef(row, ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo( + node_id - 1, kPos, dim))) = + -0.5; // previous node row++; // velocity constraint - jac.coeffRef(row, ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(node_id, kVel, dim))) = 1.0; // current node - jac.coeffRef(row, ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(node_id+1, kPos, dim))) = -1.0/t_swing_avg_; // next node - jac.coeffRef(row, ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(node_id-1, kPos, dim))) = +1.0/t_swing_avg_; // previous node + jac.coeffRef(row, ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo( + node_id, kVel, dim))) = 1.0; // current node + jac.coeffRef(row, ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo( + node_id + 1, kPos, dim))) = + -1.0 / t_swing_avg_; // next node + jac.coeffRef(row, ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo( + node_id - 1, kPos, dim))) = + +1.0 / t_swing_avg_; // previous node row++; } } diff --git a/towr/src/terrain_constraint.cc b/towr/src/terrain_constraint.cc index 8c0bdb078..fbed5c9a6 100644 --- a/towr/src/terrain_constraint.cc +++ b/towr/src/terrain_constraint.cc @@ -29,51 +29,46 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include - namespace towr { - -TerrainConstraint::TerrainConstraint (const HeightMap::Ptr& terrain, - std::string ee_motion) - :ConstraintSet(kSpecifyLater, "terrain-" + ee_motion) +TerrainConstraint::TerrainConstraint(const HeightMap::Ptr& terrain, + std::string ee_motion) + : ConstraintSet(kSpecifyLater, "terrain-" + ee_motion) { ee_motion_id_ = ee_motion; - terrain_ = terrain; + terrain_ = terrain; } -void -TerrainConstraint::InitVariableDependedQuantities (const VariablesPtr& x) +void TerrainConstraint::InitVariableDependedQuantities(const VariablesPtr& x) { ee_motion_ = x->GetComponent(ee_motion_id_); // skip first node, b/c already constrained by initial stance - for (int id=1; idGetNodes().size(); ++id) + for (int id = 1; id < ee_motion_->GetNodes().size(); ++id) node_ids_.push_back(id); int constraint_count = node_ids_.size(); SetRows(constraint_count); } -Eigen::VectorXd -TerrainConstraint::GetValues () const +Eigen::VectorXd TerrainConstraint::GetValues() const { VectorXd g(GetRows()); auto nodes = ee_motion_->GetNodes(); - int row = 0; + int row = 0; for (int id : node_ids_) { Vector3d p = nodes.at(id).p(); - g(row++) = p.z() - terrain_->GetHeight(p.x(), p.y()); + g(row++) = p.z() - terrain_->GetHeight(p.x(), p.y()); } return g; } -TerrainConstraint::VecBound -TerrainConstraint::GetBounds () const +TerrainConstraint::VecBound TerrainConstraint::GetBounds() const { VecBound bounds(GetRows()); - double max_distance_above_terrain = 1e20; // [m] + double max_distance_above_terrain = 1e20; // [m] int row = 0; for (int id : node_ids_) { @@ -87,20 +82,23 @@ TerrainConstraint::GetBounds () const return bounds; } -void -TerrainConstraint::FillJacobianBlock (std::string var_set, Jacobian& jac) const +void TerrainConstraint::FillJacobianBlock(std::string var_set, + Jacobian& jac) const { if (var_set == ee_motion_->GetName()) { auto nodes = ee_motion_->GetNodes(); - int row = 0; + int row = 0; for (int id : node_ids_) { - int idx = ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(id, kPos, Z)); + int idx = + ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(id, kPos, Z)); jac.coeffRef(row, idx) = 1.0; Vector3d p = nodes.at(id).p(); - for (auto dim : {X,Y}) { - int idx = ee_motion_->GetOptIndex(NodesVariables::NodeValueInfo(id, kPos, dim)); - jac.coeffRef(row, idx) = -terrain_->GetDerivativeOfHeightWrt(To2D(dim), p.x(), p.y()); + for (auto dim : {X, Y}) { + int idx = ee_motion_->GetOptIndex( + NodesVariables::NodeValueInfo(id, kPos, dim)); + jac.coeffRef(row, idx) = + -terrain_->GetDerivativeOfHeightWrt(To2D(dim), p.x(), p.y()); } row++; } diff --git a/towr/src/time_discretization_constraint.cc b/towr/src/time_discretization_constraint.cc index 15c4e3707..3c8acdf7b 100644 --- a/towr/src/time_discretization_constraint.cc +++ b/towr/src/time_discretization_constraint.cc @@ -33,37 +33,35 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. namespace towr { - -TimeDiscretizationConstraint::TimeDiscretizationConstraint (double T, double dt, - std::string name) - :ConstraintSet(kSpecifyLater, name) +TimeDiscretizationConstraint::TimeDiscretizationConstraint(double T, double dt, + std::string name) + : ConstraintSet(kSpecifyLater, name) { double t = 0.0; - dts_ = {t}; + dts_ = {t}; - for (int i=0; iGetComponent(id::EESchedule(ee_)); } -Eigen::VectorXd -TotalDurationConstraint::GetValues () const +Eigen::VectorXd TotalDurationConstraint::GetValues() const { VectorXd g = VectorXd::Zero(GetRows()); - g(0) = phase_durations_->GetValues().sum(); // attention: excludes last duration + g(0) = + phase_durations_->GetValues().sum(); // attention: excludes last duration return g; } -TotalDurationConstraint::VecBound -TotalDurationConstraint::GetBounds () const +TotalDurationConstraint::VecBound TotalDurationConstraint::GetBounds() const { // TODO hacky and should be fixed // since last phase is not optimized over these hardcoded numbers go here double min_duration_last_phase = 0.2; - return VecBound(GetRows(), ifopt::Bounds(0.1, T_total_-min_duration_last_phase)); + return VecBound(GetRows(), + ifopt::Bounds(0.1, T_total_ - min_duration_last_phase)); } -void -TotalDurationConstraint::FillJacobianBlock (std::string var_set, Jacobian& jac) const +void TotalDurationConstraint::FillJacobianBlock(std::string var_set, + Jacobian& jac) const { if (var_set == phase_durations_->GetName()) - for (int col=0; colGetRows(); ++col) + for (int col = 0; col < phase_durations_->GetRows(); ++col) jac.coeffRef(0, col) = 1.0; } -} // namespace towr +} // namespace towr diff --git a/towr/test/dynamic_constraint_test.cc b/towr/test/dynamic_constraint_test.cc index cc26ff29d..957e25d00 100644 --- a/towr/test/dynamic_constraint_test.cc +++ b/towr/test/dynamic_constraint_test.cc @@ -36,7 +36,6 @@ namespace towr { using VectorXd = Eigen::VectorXd; - TEST(DynamicConstraintTest, UpdateConstraintValues) { // add dynamic constraint test diff --git a/towr/test/dynamic_model_test.cc b/towr/test/dynamic_model_test.cc index 143f8aca0..5e47a5b69 100644 --- a/towr/test/dynamic_model_test.cc +++ b/towr/test/dynamic_model_test.cc @@ -48,4 +48,4 @@ TEST(DynamicModelTest, TestRotations) // update test } -} /* namespace xpp */ +} // namespace towr diff --git a/towr/test/hopper_example.cc b/towr/test/hopper_example.cc index b7df779a7..42ea249c4 100644 --- a/towr/test/hopper_example.cc +++ b/towr/test/hopper_example.cc @@ -30,10 +30,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include #include - +#include +#include using namespace towr; @@ -64,7 +63,8 @@ int main() // First we define the initial phase durations, that can however be changed // by the optimizer. The number of swing and stance phases however is fixed. // alternating stance and swing: ____-----_____-----_____-----_____ - formulation.params_.ee_phase_durations_.push_back({0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.2}); + formulation.params_.ee_phase_durations_.push_back( + {0.4, 0.2, 0.4, 0.2, 0.4, 0.2, 0.2}); formulation.params_.ee_in_contact_at_start_.push_back(true); // Initialize the nonlinear-programming problem with the variables, @@ -85,7 +85,8 @@ int main() // Choose ifopt solver (IPOPT or SNOPT), set some parameters and solve. // solver->SetOption("derivative_test", "first-order"); auto solver = std::make_shared(); - solver->SetOption("jacobian_approximation", "exact"); // "finite difference-values" + solver->SetOption("jacobian_approximation", + "exact"); // "finite difference-values" solver->SetOption("max_cpu_time", 20.0); solver->Solve(nlp); @@ -95,28 +96,31 @@ int main() // variables and query their values at specific times: using namespace std; cout.precision(2); - nlp.PrintCurrent(); // view variable-set, constraint violations, indices,... + nlp.PrintCurrent(); // view variable-set, constraint violations, indices,... cout << fixed; cout << "\n====================\nMonoped trajectory:\n====================\n"; double t = 0.0; - while (t<=solution.base_linear_->GetTotalTime() + 1e-5) { + while (t <= solution.base_linear_->GetTotalTime() + 1e-5) { cout << "t=" << t << "\n"; cout << "Base linear position x,y,z: \t"; - cout << solution.base_linear_->GetPoint(t).p().transpose() << "\t[m]" << endl; + cout << solution.base_linear_->GetPoint(t).p().transpose() << "\t[m]" + << endl; cout << "Base Euler roll, pitch, yaw: \t"; Eigen::Vector3d rad = solution.base_angular_->GetPoint(t).p(); - cout << (rad/M_PI*180).transpose() << "\t[deg]" << endl; + cout << (rad / M_PI * 180).transpose() << "\t[deg]" << endl; cout << "Foot position x,y,z: \t"; - cout << solution.ee_motion_.at(0)->GetPoint(t).p().transpose() << "\t[m]" << endl; + cout << solution.ee_motion_.at(0)->GetPoint(t).p().transpose() << "\t[m]" + << endl; cout << "Contact force x,y,z: \t"; - cout << solution.ee_force_.at(0)->GetPoint(t).p().transpose() << "\t[N]" << endl; + cout << solution.ee_force_.at(0)->GetPoint(t).p().transpose() << "\t[N]" + << endl; bool contact = solution.phase_durations_.at(0)->IsContactPhase(t); - std::string foot_in_contact = contact? "yes" : "no"; + std::string foot_in_contact = contact ? "yes" : "no"; cout << "Foot in contact: \t" + foot_in_contact << endl; cout << endl; diff --git a/towr_ros/include/towr_ros/topic_names.h b/towr_ros/include/towr_ros/topic_names.h index c2f336074..50e601243 100644 --- a/towr_ros/include/towr_ros/topic_names.h +++ b/towr_ros/include/towr_ros/topic_names.h @@ -43,7 +43,6 @@ static const std::string nlp_iterations_count("/towr/nlp_iterations_count"); // the base topic names of each nlp iteration static const std::string nlp_iterations_name("/towr/nlp_iterations_name"); -} // namespace towr_msgs - +} // namespace towr_msgs #endif /* TOWR_ROS_TOPIC_NAMES_H_ */ diff --git a/towr_ros/include/towr_ros/towr_ros_interface.h b/towr_ros/include/towr_ros/towr_ros_interface.h index 473d86552..0eccdf781 100644 --- a/towr_ros/include/towr_ros/towr_ros_interface.h +++ b/towr_ros/include/towr_ros/towr_ros_interface.h @@ -35,18 +35,16 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include -#include -#include #include +#include +#include +#include -#include #include - +#include namespace towr { - /** * @brief Base class to interface TOWR with a ROS GUI and RVIZ. * @@ -55,14 +53,14 @@ namespace towr { * interface is TowrRosApp. */ class TowrRosInterface { -public: + public: using XppVec = std::vector; using TowrCommandMsg = towr_ros::TowrCommand; using Vector3d = Eigen::Vector3d; -protected: - TowrRosInterface (); - virtual ~TowrRosInterface () = default; + protected: + TowrRosInterface(); + virtual ~TowrRosInterface() = default; /** * @brief Sets the base state and end-effector position. @@ -76,7 +74,8 @@ class TowrRosInterface { * When formulating your own application, here you can set your specific * set of constraints and variables. */ - virtual Parameters GetTowrParameters(int n_ee, const TowrCommandMsg& msg) const = 0; + virtual Parameters GetTowrParameters(int n_ee, + const TowrCommandMsg& msg) const = 0; /** * @brief Sets the parameters of the nonlinear programming solver IPOPT. @@ -84,13 +83,13 @@ class TowrRosInterface { */ virtual void SetIpoptParameters(const TowrCommandMsg& msg) = 0; - NlpFormulation formulation_; ///< the default formulation, can be adapted - ifopt::IpoptSolver::Ptr solver_; ///< NLP solver, could also use SNOPT. + NlpFormulation formulation_; ///< the default formulation, can be adapted + ifopt::IpoptSolver::Ptr solver_; ///< NLP solver, could also use SNOPT. -private: - SplineHolder solution; ///< the solution splines linked to the opt-variables. - ifopt::Problem nlp_; ///< the actual nonlinear program to be solved. - double visualization_dt_; ///< duration between two rviz visualization states. + private: + SplineHolder solution; ///< the solution splines linked to the opt-variables. + ifopt::Problem nlp_; ///< the actual nonlinear program to be solved. + double visualization_dt_; ///< duration between 2 rviz visualization states. ::ros::Subscriber user_command_sub_; ::ros::Publisher initial_state_pub_; @@ -100,15 +99,16 @@ class TowrRosInterface { XppVec GetTrajectory() const; virtual BaseState GetGoalState(const TowrCommandMsg& msg) const; void PublishInitialState(); - std::vectorGetIntermediateSolutions(); - xpp_msgs::RobotParameters BuildRobotParametersMsg(const RobotModel& model) const; + std::vector GetIntermediateSolutions(); + xpp_msgs::RobotParameters BuildRobotParametersMsg( + const RobotModel& model) const; void SaveOptimizationAsRosbag(const std::string& bag_name, const xpp_msgs::RobotParameters& robot_params, const TowrCommandMsg user_command_msg, - bool include_iterations=false); - void SaveTrajectoryInRosbag (rosbag::Bag&, - const std::vector& traj, - const std::string& topic) const; + bool include_iterations = false); + void SaveTrajectoryInRosbag(rosbag::Bag&, + const std::vector& traj, + const std::string& topic) const; }; } /* namespace towr */ diff --git a/towr_ros/include/towr_ros/towr_user_interface.h b/towr_ros/include/towr_ros/towr_user_interface.h index 3694e1a1e..cfab09d99 100644 --- a/towr_ros/include/towr_ros/towr_user_interface.h +++ b/towr_ros/include/towr_ros/towr_user_interface.h @@ -43,12 +43,12 @@ namespace towr { * keyboard input into a goal state), which terrain to visualize, etc. */ class TowrUserInterface { -public: + public: /** * @brief Constructs default object to interact with framework. */ - TowrUserInterface (); - virtual ~TowrUserInterface () = default; + TowrUserInterface(); + virtual ~TowrUserInterface() = default; /** * Called whenever a keyboard key is pressed. @@ -56,9 +56,8 @@ class TowrUserInterface { */ void CallbackKey(int c); -private: - ::ros::Publisher user_command_pub_; ///< the output message to TOWR. - + private: + ::ros::Publisher user_command_pub_; ///< the output message to TOWR. void PublishCommand(); diff --git a/towr_ros/include/towr_ros/towr_xpp_ee_map.h b/towr_ros/include/towr_ros/towr_xpp_ee_map.h index 9642d45a5..3a777187a 100644 --- a/towr_ros/include/towr_ros/towr_xpp_ee_map.h +++ b/towr_ros/include/towr_ros/towr_xpp_ee_map.h @@ -35,44 +35,33 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #ifndef TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_TOWR_XPP_EE_MAP_H_ #define TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_TOWR_XPP_EE_MAP_H_ -#include #include #include - +#include namespace towr { /** Mapping endeffector IDs */ -static std::map biped_to_xpp_id = -{ +static std::map biped_to_xpp_id = { {L, xpp::biped::L}, {R, xpp::biped::R}, }; -static std::map quad_to_xpp_id = -{ +static std::map quad_to_xpp_id = { {LF, xpp::quad::LF}, {RF, xpp::quad::RF}, {LH, xpp::quad::LH}, - {RH, xpp::quad::RH} -}; - + {RH, xpp::quad::RH}}; /** Mapping endeffector names */ -static std::map biped_to_name = -{ - {L, "Left" }, - {R, "Right"} -}; - -static std::map quad_to_name = -{ - {LF, "Left-Front" }, - {RF, "Right-Front"}, - {LH, "Left-Hind" }, - {RH, "Right-Hind" } -}; +static std::map biped_to_name = {{L, "Left"}, + {R, "Right"}}; +static std::map quad_to_name = { + {LF, "Left-Front"}, + {RF, "Right-Front"}, + {LH, "Left-Hind"}, + {RH, "Right-Hind"}}; /** * Converts endeffector IDs of towr into the corresponding number used in xpp. @@ -82,8 +71,8 @@ static std::map quad_to_name = * @return corresponding endeffector and string name in the xpp domain. */ -static std::pair -ToXppEndeffector(int number_of_ee, int towr_ee_id) +static std::pair ToXppEndeffector( + int number_of_ee, int towr_ee_id) { std::pair ee; @@ -93,19 +82,19 @@ ToXppEndeffector(int number_of_ee, int towr_ee_id) ee.second = "E0"; break; case 2: { - auto id = static_cast(towr_ee_id); + auto id = static_cast(towr_ee_id); ee.first = biped_to_xpp_id.at(id); ee.second = biped_to_name.at(id); break; } case 4: { - auto id = static_cast(towr_ee_id); + auto id = static_cast(towr_ee_id); ee.first = quad_to_xpp_id.at(id); ee.second = quad_to_name.at(id); break; } default: - assert(false); // endeffector mapping not defined + assert(false); // endeffector mapping not defined break; } @@ -126,6 +115,6 @@ static xpp::StateLinXd ToXpp(const towr::State& towr) return xpp; } -} // namespace towr +} // namespace towr #endif /* TOWR_TOWR_ROS_INCLUDE_TOWR_ROS_TOWR_XPP_EE_MAP_H_ */ diff --git a/towr_ros/src/goal_pose_publisher.cc b/towr_ros/src/goal_pose_publisher.cc index 93f4a83e3..c4f066ecf 100644 --- a/towr_ros/src/goal_pose_publisher.cc +++ b/towr_ros/src/goal_pose_publisher.cc @@ -27,8 +27,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. ******************************************************************************/ -#include #include +#include #include #include @@ -36,7 +36,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include // listen to goal state #include - namespace towr { static ros::Publisher rviz_pub; @@ -45,36 +44,35 @@ void UserCommandCallback(const towr_ros::TowrCommand& msg_in) { // get which terrain auto terrain_id = static_cast(msg_in.terrain); - auto terrain_ = HeightMap::MakeTerrain(terrain_id); + auto terrain_ = HeightMap::MakeTerrain(terrain_id); geometry_msgs::PoseStamped goal_msg; goal_msg.header.frame_id = "world"; // visualize goal z state on terrain. - double x = msg_in.goal_lin.pos.x; - double y = msg_in.goal_lin.pos.y; + double x = msg_in.goal_lin.pos.x; + double y = msg_in.goal_lin.pos.y; goal_msg.pose.position.x = x; goal_msg.pose.position.y = y; goal_msg.pose.position.z = terrain_->GetHeight(x, y); // orientation according to message - Eigen::Quaterniond q = xpp::GetQuaternionFromEulerZYX(msg_in.goal_ang.pos.z, - msg_in.goal_ang.pos.y, - msg_in.goal_ang.pos.x); + Eigen::Quaterniond q = xpp::GetQuaternionFromEulerZYX( + msg_in.goal_ang.pos.z, msg_in.goal_ang.pos.y, msg_in.goal_ang.pos.x); goal_msg.pose.orientation = xpp::Convert::ToRos(q); rviz_pub.publish(goal_msg); } -} // namespace towr +} // namespace towr -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { ros::init(argc, argv, "goal_pose_publisher"); ros::NodeHandle n; ros::Subscriber goal_sub; - goal_sub = n.subscribe(towr_msgs::user_command, 1, towr::UserCommandCallback); + goal_sub = n.subscribe(towr_msgs::user_command, 1, towr::UserCommandCallback); towr::rviz_pub = n.advertise("xpp/goal", 1); ros::spin(); diff --git a/towr_ros/src/rosbag_geom_msg_extractor.cc b/towr_ros/src/rosbag_geom_msg_extractor.cc index 7dfdd2b68..73104ba6d 100644 --- a/towr_ros/src/rosbag_geom_msg_extractor.cc +++ b/towr_ros/src/rosbag_geom_msg_extractor.cc @@ -32,8 +32,8 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include -#include #include +#include #include #include @@ -45,9 +45,9 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * * See towr/matlab/plot_rosbag.m for an example of how to open these. */ -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { - if (argc==1) { + if (argc == 1) { std::cerr << "Error: Please enter path to bag file\n"; return 0; } @@ -70,18 +70,19 @@ int main(int argc, char *argv[]) rosbag::Bag bag_w; bag_w.open("/home/winklera/Desktop/matlab_rdy.bag", rosbag::bagmode::Write); - BOOST_FOREACH(rosbag::MessageInstance const m, view) - { - ros::Time t = m.getTime(); + BOOST_FOREACH (rosbag::MessageInstance const m, view) { + ros::Time t = m.getTime(); auto state_msg = m.instantiate(); bag_w.write("base_pose", t, state_msg->base.pose); bag_w.write("base_acc", t, state_msg->base.accel.linear); int n_feet = state_msg->ee_motion.size(); - for (int i=0; iee_motion.at(i).pos); - bag_w.write("foot_force_"+std::to_string(i), t, state_msg->ee_forces.at(i)); + for (int i = 0; i < n_feet; ++i) { + bag_w.write("foot_pos_" + std::to_string(i), t, + state_msg->ee_motion.at(i).pos); + bag_w.write("foot_force_" + std::to_string(i), t, + state_msg->ee_forces.at(i)); } } diff --git a/towr_ros/src/rosbag_traj_combiner.cc b/towr_ros/src/rosbag_traj_combiner.cc index df2dbe6e2..f8fa70a8e 100644 --- a/towr_ros/src/rosbag_traj_combiner.cc +++ b/towr_ros/src/rosbag_traj_combiner.cc @@ -33,13 +33,13 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include +#include #include #include -#include #include -#include #include +#include /** * Takes a ROS bag of optimization results (with intermediate iterations), and @@ -47,54 +47,55 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. * * (used for RA-L submission video). */ -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { ros::init(argc, argv, "rosbag_trajectory_combiner"); std::string name = "/home/winklera/bags/optimal_traj"; rosbag::Bag bag_r; - bag_r.open(name+".bag", rosbag::bagmode::Read); + bag_r.open(name + ".bag", rosbag::bagmode::Read); ROS_INFO_STREAM("Reading from bag " + bag_r.getFileName()); - // get number of iterations in bag file int n_opt_iterations = 0; - rosbag::View view1(bag_r, rosbag::TopicQuery(towr_msgs::nlp_iterations_count)); - BOOST_FOREACH(rosbag::MessageInstance const m, view1) { + rosbag::View view1(bag_r, + rosbag::TopicQuery(towr_msgs::nlp_iterations_count)); + BOOST_FOREACH (rosbag::MessageInstance const m, view1) { std_msgs::Int32::ConstPtr i = m.instantiate(); - n_opt_iterations = i->data; + n_opt_iterations = i->data; } // select which iterations (message topics) to be included in bag file std::vector topics; - ROS_INFO_STREAM("Detected " + std::to_string(n_opt_iterations) + " iterations"); - int n_visualizations = 5; // total number of visualizations is fixed - int frequency = std::floor(n_opt_iterations/n_visualizations); - - for (int i=0; i t_iter; - double duration = view.getEndTime().toSec(); // duration of the trajectory - for (int i=0; i +#include #include #include -#include - namespace towr { @@ -45,37 +44,40 @@ void UserCommandCallback(const towr_ros::TowrCommand& msg_in) { // get which terrain auto terrain_id = static_cast(msg_in.terrain); - auto terrain_ = HeightMap::MakeTerrain(terrain_id); + auto terrain_ = HeightMap::MakeTerrain(terrain_id); // x-y area patch that should be drawn in rviz - double dxy = 0.06; + double dxy = 0.06; double x_min = -1.0; - double x_max = 4.0; + double x_max = 4.0; double y_min = -1.0; - double y_max = 1.0; + double y_max = 1.0; visualization_msgs::Marker m; - int id = 0; - m.type = visualization_msgs::Marker::CUBE; - m.scale.z = 0.003; - m.ns = "terrain"; + int id = 0; + m.type = visualization_msgs::Marker::CUBE; + m.scale.z = 0.003; + m.ns = "terrain"; m.header.frame_id = "world"; - m.color.r = 245./355; m.color.g = 222./355; m.color.b = 179./355; // wheat - m.color.a = 0.65; + m.color.r = 245. / 355; + m.color.g = 222. / 355; + m.color.b = 179. / 355; // wheat + m.color.a = 0.65; visualization_msgs::MarkerArray msg; - double x = x_min; + double x = x_min; while (x < x_max) { double y = y_min; while (y < y_max) { // position m.pose.position.x = x; m.pose.position.y = y; - m.pose.position.z = terrain_->GetHeight(x,y); + m.pose.position.z = terrain_->GetHeight(x, y); // orientation Eigen::Vector3d n = terrain_->GetNormalizedBasis(HeightMap::Normal, x, y); - Eigen::Quaterniond q = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0,0,1), n); + Eigen::Quaterniond q = + Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d(0, 0, 1), n); m.pose.orientation.w = q.w(); m.pose.orientation.x = q.x(); m.pose.orientation.y = q.y(); @@ -83,9 +85,8 @@ void UserCommandCallback(const towr_ros::TowrCommand& msg_in) // enlarge surface-path when tilting double gain = 1.5; - m.scale.x = (1+gain*n.cwiseAbs().x())*dxy; - m.scale.y = (1+gain*n.cwiseAbs().y())*dxy; - + m.scale.x = (1 + gain * n.cwiseAbs().x()) * dxy; + m.scale.y = (1 + gain * n.cwiseAbs().y()) * dxy; m.id = id++; msg.markers.push_back(m); @@ -98,17 +99,18 @@ void UserCommandCallback(const towr_ros::TowrCommand& msg_in) rviz_pub.publish(msg); } -} // namespace towr +} // namespace towr -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { ros::init(argc, argv, "rviz_terrain_visualizer"); ros::NodeHandle n; ros::Subscriber goal_sub; - goal_sub = n.subscribe(towr_msgs::user_command, 1, towr::UserCommandCallback); - towr::rviz_pub = n.advertise("xpp/terrain", 1); + goal_sub = n.subscribe(towr_msgs::user_command, 1, towr::UserCommandCallback); + towr::rviz_pub = + n.advertise("xpp/terrain", 1); ros::spin(); diff --git a/towr_ros/src/towr_ros_app.cc b/towr_ros/src/towr_ros_app.cc index 11b7bc597..7366c9a44 100644 --- a/towr_ros/src/towr_ros_app.cc +++ b/towr_ros/src/towr_ros_app.cc @@ -30,7 +30,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include #include - namespace towr { /** @@ -40,27 +39,33 @@ namespace towr { * blocks provided in TOWR and following the example below. */ class TowrRosApp : public TowrRosInterface { -public: + public: /** * @brief Sets the feet to nominal position on flat ground and base above. */ void SetTowrInitialState() override { - auto nominal_stance_B = formulation_.model_.kinematic_model_->GetNominalStanceInBase(); - - double z_ground = 0.0; - formulation_.initial_ee_W_ = nominal_stance_B; - std::for_each(formulation_.initial_ee_W_.begin(), formulation_.initial_ee_W_.end(), - [&](Vector3d& p){ p.z() = z_ground; } // feet at 0 height + auto nominal_stance_B = + formulation_.model_.kinematic_model_->GetNominalStanceInBase(); + + double z_ground = 0.0; + formulation_.initial_ee_W_ = nominal_stance_B; + std::for_each(formulation_.initial_ee_W_.begin(), + formulation_.initial_ee_W_.end(), + [&](Vector3d& p) { + p.z() = z_ground; + } // feet at 0 height ); - formulation_.initial_base_.lin.at(kPos).z() = - nominal_stance_B.front().z() + z_ground; + formulation_.initial_base_.lin.at(kPos).z() = + -nominal_stance_B.front().z() + z_ground; } /** * @brief Sets the parameters required to formulate the TOWR problem. */ - Parameters GetTowrParameters(int n_ee, const TowrCommandMsg& msg) const override + Parameters GetTowrParameters(int n_ee, + const TowrCommandMsg& msg) const override { Parameters params; @@ -70,9 +75,11 @@ class TowrRosApp : public TowrRosInterface { auto gait_gen_ = GaitGenerator::MakeGaitGenerator(n_ee); auto id_gait = static_cast(msg.gait); gait_gen_->SetCombo(id_gait); - for (int ee=0; eeGetPhaseDurations(msg.total_duration, ee)); - params.ee_in_contact_at_start_.push_back(gait_gen_->IsInContactAtStart(ee)); + for (int ee = 0; ee < n_ee; ++ee) { + params.ee_phase_durations_.push_back( + gait_gen_->GetPhaseDurations(msg.total_duration, ee)); + params.ee_in_contact_at_start_.push_back( + gait_gen_->IsInContactAtStart(ee)); } // Here you can also add other constraints or change parameters @@ -92,7 +99,7 @@ class TowrRosApp : public TowrRosInterface { void SetIpoptParameters(const TowrCommandMsg& msg) override { // the HA-L solvers are alot faster, so consider installing and using - solver_->SetOption("linear_solver", "mumps"); // ma27, ma57 + solver_->SetOption("linear_solver", "mumps"); // ma27, ma57 // Analytically defining the derivatives in IFOPT as we do it, makes the // problem a lot faster. However, if this becomes too difficult, we can also @@ -100,7 +107,8 @@ class TowrRosApp : public TowrRosInterface { // this uses numerical derivatives for ALL constraints, there doesn't yet // exist an option to turn on numerical derivatives for only some constraint // sets. - solver_->SetOption("jacobian_approximation", "exact"); // finite difference-values + solver_->SetOption("jacobian_approximation", + "exact"); // finite difference-values // This is a great to test if the analytical derivatives implemented in are // correct. Some derivatives that are correct are still flagged, showing a @@ -117,10 +125,9 @@ class TowrRosApp : public TowrRosInterface { } }; -} // namespace towr - +} // namespace towr -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { ros::init(argc, argv, "my_towr_ros_app"); towr::TowrRosApp towr_app; diff --git a/towr_ros/src/towr_ros_interface.cc b/towr_ros/src/towr_ros_interface.cc index d4accd93e..c545da2c8 100644 --- a/towr_ros/src/towr_ros_interface.cc +++ b/towr_ros/src/towr_ros_interface.cc @@ -31,39 +31,36 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include #include +#include +#include #include #include #include #include - namespace towr { - -TowrRosInterface::TowrRosInterface () +TowrRosInterface::TowrRosInterface() { ::ros::NodeHandle n; user_command_sub_ = n.subscribe(towr_msgs::user_command, 1, &TowrRosInterface::UserCommandCallback, this); - initial_state_pub_ = n.advertise - (xpp_msgs::robot_state_desired, 1); + initial_state_pub_ = n.advertise( + xpp_msgs::robot_state_desired, 1); - robot_parameters_pub_ = n.advertise - (xpp_msgs::robot_parameters, 1); + robot_parameters_pub_ = + n.advertise(xpp_msgs::robot_parameters, 1); solver_ = std::make_shared(); visualization_dt_ = 0.01; } -BaseState -TowrRosInterface::GetGoalState(const TowrCommandMsg& msg) const +BaseState TowrRosInterface::GetGoalState(const TowrCommandMsg& msg) const { BaseState goal; goal.lin.at(kPos) = xpp::Convert::ToXpp(msg.goal_lin.pos); @@ -74,20 +71,19 @@ TowrRosInterface::GetGoalState(const TowrCommandMsg& msg) const return goal; } -void -TowrRosInterface::UserCommandCallback(const TowrCommandMsg& msg) +void TowrRosInterface::UserCommandCallback(const TowrCommandMsg& msg) { // robot model - formulation_.model_ = RobotModel(static_cast(msg.robot)); + formulation_.model_ = RobotModel(static_cast(msg.robot)); auto robot_params_msg = BuildRobotParametersMsg(formulation_.model_); robot_parameters_pub_.publish(robot_params_msg); // terrain - auto terrain_id = static_cast(msg.terrain); + auto terrain_id = static_cast(msg.terrain); formulation_.terrain_ = HeightMap::MakeTerrain(terrain_id); int n_ee = formulation_.model_.kinematic_model_->GetNumberOfEndeffectors(); - formulation_.params_ = GetTowrParameters(n_ee, msg); + formulation_.params_ = GetTowrParameters(n_ee, msg); formulation_.final_base_ = GetGoalState(msg); SetTowrInitialState(); @@ -115,45 +111,46 @@ TowrRosInterface::UserCommandCallback(const TowrCommandMsg& msg) // playback using terminal commands if (msg.replay_trajectory || msg.play_initialization || msg.optimize) { - int success = system(("rosbag play --topics " - + xpp_msgs::robot_state_desired + " " - + xpp_msgs::terrain_info - + " -r " + std::to_string(msg.replay_speed) - + " --quiet " + bag_file).c_str()); + int success = + system(("rosbag play --topics " + xpp_msgs::robot_state_desired + " " + + xpp_msgs::terrain_info + " -r " + + std::to_string(msg.replay_speed) + " --quiet " + bag_file) + .c_str()); } if (msg.plot_trajectory) { - int success = system(("killall rqt_bag; rqt_bag " + bag_file + "&").c_str()); + int success = + system(("killall rqt_bag; rqt_bag " + bag_file + "&").c_str()); } // to publish entire trajectory (e.g. to send to controller) // xpp_msgs::RobotStateCartesianTrajectory xpp_msg = xpp::Convert::ToRos(GetTrajectory()); } -void -TowrRosInterface::PublishInitialState() +void TowrRosInterface::PublishInitialState() { int n_ee = formulation_.initial_ee_W_.size(); xpp::RobotStateCartesian xpp(n_ee); xpp.base_.lin.p_ = formulation_.initial_base_.lin.p(); - xpp.base_.ang.q = EulerConverter::GetQuaternionBaseToWorld(formulation_.initial_base_.ang.p()); + xpp.base_.ang.q = EulerConverter::GetQuaternionBaseToWorld( + formulation_.initial_base_.ang.p()); - for (int ee_towr=0; ee_towr -TowrRosInterface::GetIntermediateSolutions () +TowrRosInterface::GetIntermediateSolutions() { std::vector trajectories; - for (int iter=0; iterIsContactPhase(t); - state.ee_motion_.at(ee_xpp) = ToXpp(solution.ee_motion_.at(ee_towr)->GetPoint(t)); - state.ee_forces_ .at(ee_xpp) = solution.ee_force_.at(ee_towr)->GetPoint(t).p(); + state.ee_contact_.at(ee_xpp) = + solution.phase_durations_.at(ee_towr)->IsContactPhase(t); + state.ee_motion_.at(ee_xpp) = + ToXpp(solution.ee_motion_.at(ee_towr)->GetPoint(t)); + state.ee_forces_.at(ee_xpp) = + solution.ee_force_.at(ee_towr)->GetPoint(t).p(); } state.t_global_ = t; @@ -196,18 +195,20 @@ TowrRosInterface::GetTrajectory () const return trajectory; } -xpp_msgs::RobotParameters -TowrRosInterface::BuildRobotParametersMsg(const RobotModel& model) const +xpp_msgs::RobotParameters TowrRosInterface::BuildRobotParametersMsg( + const RobotModel& model) const { xpp_msgs::RobotParameters params_msg; auto max_dev_xyz = model.kinematic_model_->GetMaximumDeviationFromNominal(); - params_msg.ee_max_dev = xpp::Convert::ToRos(max_dev_xyz); + params_msg.ee_max_dev = + xpp::Convert::ToRos(max_dev_xyz); auto nominal_B = model.kinematic_model_->GetNominalStanceInBase(); - int n_ee = nominal_B.size(); - for (int ee_towr=0; ee_towr(pos)); + params_msg.nominal_ee_pos.push_back( + xpp::Convert::ToRos(pos)); params_msg.ee_names.push_back(ToXppEndeffector(n_ee, ee_towr).second); } @@ -216,26 +217,26 @@ TowrRosInterface::BuildRobotParametersMsg(const RobotModel& model) const return params_msg; } -void -TowrRosInterface::SaveOptimizationAsRosbag (const std::string& bag_name, - const xpp_msgs::RobotParameters& robot_params, - const TowrCommandMsg user_command_msg, - bool include_iterations) +void TowrRosInterface::SaveOptimizationAsRosbag( + const std::string& bag_name, const xpp_msgs::RobotParameters& robot_params, + const TowrCommandMsg user_command_msg, bool include_iterations) { rosbag::Bag bag; bag.open(bag_name, rosbag::bagmode::Write); - ::ros::Time t0(1e-6); // t=0.0 throws ROS exception + ::ros::Time t0(1e-6); // t=0.0 throws ROS exception // save the a-priori fixed optimization variables bag.write(xpp_msgs::robot_parameters, t0, robot_params); - bag.write(towr_msgs::user_command+"_saved", t0, user_command_msg); + bag.write(towr_msgs::user_command + "_saved", t0, user_command_msg); // save the trajectory of each iteration if (include_iterations) { auto trajectories = GetIntermediateSolutions(); - int n_iterations = trajectories.size(); - for (int i=0; iGetNormalizedBasis(HeightMap::Normal, ee.p_.x(), ee.p_.y()); - terrain_msg.surface_normals.push_back(xpp::Convert::ToRos(n)); + Vector3d n = formulation_.terrain_->GetNormalizedBasis( + HeightMap::Normal, ee.p_.x(), ee.p_.y()); + terrain_msg.surface_normals.push_back( + xpp::Convert::ToRos(n)); terrain_msg.friction_coeff = formulation_.terrain_->GetFrictionCoeff(); } @@ -274,4 +277,3 @@ TowrRosInterface::SaveTrajectoryInRosbag (rosbag::Bag& bag, } } /* namespace towr */ - diff --git a/towr_ros/src/towr_user_interface.cc b/towr_ros/src/towr_user_interface.cc index 6dcb1ad9b..fb1385aad 100644 --- a/towr_ros/src/towr_user_interface.cc +++ b/towr_ros/src/towr_user_interface.cc @@ -33,56 +33,73 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. #include -#include -#include -#include #include #include - +#include +#include +#include namespace towr { - -enum YCursorRows {HEADING=6, OPTIMIZE=8, VISUALIZE, INITIALIZATION, PLOT, - REPLAY_SPEED, GOAL_POS, GOAL_ORI, ROBOT, - GAIT, OPTIMIZE_GAIT, TERRAIN, DURATION, CLOSE, END}; -static constexpr int Y_STATUS = END+1; +enum YCursorRows { + HEADING = 6, + OPTIMIZE = 8, + VISUALIZE, + INITIALIZATION, + PLOT, + REPLAY_SPEED, + GOAL_POS, + GOAL_ORI, + ROBOT, + GAIT, + OPTIMIZE_GAIT, + TERRAIN, + DURATION, + CLOSE, + END +}; + +static constexpr int Y_STATUS = END + 1; static constexpr int X_KEY = 1; static constexpr int X_DESCRIPTION = 10; static constexpr int X_VALUE = 35; - -TowrUserInterface::TowrUserInterface () +TowrUserInterface::TowrUserInterface() { - printw(" ******************************************************************************\n"); + printw( + " ***********************************************************************" + "*******\n"); printw(" TOWR user interface (v1.4) \n"); printw(" \u00a9 Alexander W. Winkler \n"); printw(" https://github.com/ethz-adrl/towr\n"); - printw(" ******************************************************************************\n\n"); + printw( + " ***********************************************************************" + "*******\n\n"); ::ros::NodeHandle n; - user_command_pub_ = n.advertise(towr_msgs::user_command, 1); + user_command_pub_ = + n.advertise(towr_msgs::user_command, 1); goal_geom_.lin.p_.setZero(); goal_geom_.lin.p_ << 2.1, 0.0, 0.0; - goal_geom_.ang.p_ << 0.0, 0.0, 0.0; // roll, pitch, yaw angle applied Z->Y'->X'' - - robot_ = RobotModel::Monoped; - terrain_ = HeightMap::FlatID; - gait_combo_ = GaitGenerator::C0; - total_duration_ = 2.4; - visualize_trajectory_ = false; - plot_trajectory_ = false; - replay_speed_ = 1.0; // realtime - optimize_ = false; + // roll, pitch, yaw angle applied Z->Y'->X'' + goal_geom_.ang.p_ << 0.0, 0.0, 0.0; + + robot_ = RobotModel::Monoped; + terrain_ = HeightMap::FlatID; + gait_combo_ = GaitGenerator::C0; + total_duration_ = 2.4; + visualize_trajectory_ = false; + plot_trajectory_ = false; + replay_speed_ = 1.0; // realtime + optimize_ = false; publish_optimized_trajectory_ = false; - optimize_phase_durations_ = false; + optimize_phase_durations_ = false; PrintScreen(); } -void -TowrUserInterface::PrintScreen() const +void TowrUserInterface::PrintScreen() const { wmove(stdscr, HEADING, X_KEY); printw("Key"); @@ -147,7 +164,8 @@ TowrUserInterface::PrintScreen() const wmove(stdscr, ROBOT, X_DESCRIPTION); printw("Robot"); wmove(stdscr, ROBOT, X_VALUE); - printw("%s\n", robot_names.at(static_cast(robot_)).c_str()); + printw("%s\n", + robot_names.at(static_cast(robot_)).c_str()); wmove(stdscr, GAIT, X_KEY); printw("g"); @@ -161,14 +179,15 @@ TowrUserInterface::PrintScreen() const wmove(stdscr, OPTIMIZE_GAIT, X_DESCRIPTION); printw("Optimize gait"); wmove(stdscr, OPTIMIZE_GAIT, X_VALUE); - optimize_phase_durations_? printw("On\n") : printw("off\n"); + optimize_phase_durations_ ? printw("On\n") : printw("off\n"); wmove(stdscr, TERRAIN, X_KEY); printw("t"); wmove(stdscr, TERRAIN, X_DESCRIPTION); printw("Terrain"); wmove(stdscr, TERRAIN, X_VALUE); - printw("%s\n", terrain_names.at(static_cast(terrain_)).c_str()); + printw("%s\n", + terrain_names.at(static_cast(terrain_)).c_str()); wmove(stdscr, DURATION, X_KEY); printw("+/-"); @@ -185,11 +204,10 @@ TowrUserInterface::PrintScreen() const printw("-"); } -void -TowrUserInterface::CallbackKey (int c) +void TowrUserInterface::CallbackKey(int c) { - const static double d_lin = 0.1; // [m] - const static double d_ang = 0.25; // [rad] + const static double d_lin = 0.1; // [m] + const static double d_ang = 0.25; // [rad] switch (c) { case KEY_RIGHT: @@ -205,30 +223,30 @@ TowrUserInterface::CallbackKey (int c) goal_geom_.lin.p_.y() -= d_lin; break; case KEY_PPAGE: - goal_geom_.lin.p_.z() += 0.5*d_lin; + goal_geom_.lin.p_.z() += 0.5 * d_lin; break; case KEY_NPAGE: - goal_geom_.lin.p_.z() -= 0.5*d_lin; + goal_geom_.lin.p_.z() -= 0.5 * d_lin; break; // desired goal orientations case '4': - goal_geom_.ang.p_.x() -= d_ang; // roll- + goal_geom_.ang.p_.x() -= d_ang; // roll- break; case '6': - goal_geom_.ang.p_.x() += d_ang; // roll+ + goal_geom_.ang.p_.x() += d_ang; // roll+ break; case '8': - goal_geom_.ang.p_.y() += d_ang; // pitch+ + goal_geom_.ang.p_.y() += d_ang; // pitch+ break; case '2': - goal_geom_.ang.p_.y() -= d_ang; // pitch- + goal_geom_.ang.p_.y() -= d_ang; // pitch- break; case '1': - goal_geom_.ang.p_.z() += d_ang; // yaw+ + goal_geom_.ang.p_.z() += d_ang; // yaw+ break; case '9': - goal_geom_.ang.p_.z() -= d_ang; // yaw- + goal_geom_.ang.p_.z() -= d_ang; // yaw- break; // terrains @@ -237,7 +255,8 @@ TowrUserInterface::CallbackKey (int c) break; case 'g': - gait_combo_ = AdvanceCircularBuffer(gait_combo_, GaitGenerator::COMBO_COUNT); + gait_combo_ = + AdvanceCircularBuffer(gait_combo_, GaitGenerator::COMBO_COUNT); break; case 'r': @@ -247,21 +266,20 @@ TowrUserInterface::CallbackKey (int c) // duration case '+': total_duration_ += 0.2; - break; + break; case '-': total_duration_ -= 0.2; - break; + break; case '\'': replay_speed_ += 0.1; - break; + break; case ';': replay_speed_ -= 0.1; - break; + break; case 'y': optimize_phase_durations_ = !optimize_phase_durations_; break; - case 'o': optimize_ = true; wmove(stdscr, Y_STATUS, 0); @@ -280,12 +298,14 @@ TowrUserInterface::CallbackKey (int c) case 'p': plot_trajectory_ = true; wmove(stdscr, Y_STATUS, 0); - printw("In rqt_bag: right-click on xpp/state_des -> View -> Plot.\n" - "Then expand the values you wish to plot on the right\n"); + printw( + "In rqt_bag: right-click on xpp/state_des -> View -> Plot.\n" + "Then expand the values you wish to plot on the right\n"); break; case 'q': printw("Closing user interface\n"); - ros::shutdown(); break; + ros::shutdown(); + break; default: break; } @@ -313,34 +333,31 @@ void TowrUserInterface::PublishCommand() PrintScreen(); - optimize_ = false; - visualize_trajectory_ = false; - plot_trajectory_ = false; - play_initialization_ = false; + optimize_ = false; + visualize_trajectory_ = false; + plot_trajectory_ = false; + play_initialization_ = false; publish_optimized_trajectory_ = false; } int TowrUserInterface::AdvanceCircularBuffer(int& curr, int max) const { - return curr==(max-1)? 0 : curr+1; + return curr == (max - 1) ? 0 : curr + 1; } -void -TowrUserInterface::PrintVector(const Eigen::Vector3d& v) const +void TowrUserInterface::PrintVector(const Eigen::Vector3d& v) const { printw("%.2f %.2f %.2f", v.x(), v.y(), v.z()); } -void -TowrUserInterface::PrintVector2D(const Eigen::Vector2d& v) const +void TowrUserInterface::PrintVector2D(const Eigen::Vector2d& v) const { printw("%.2f %.2f", v.x(), v.y()); } } /* namespace towr */ - -int main(int argc, char *argv[]) +int main(int argc, char* argv[]) { ros::init(argc, argv, "towr_user_iterface"); @@ -351,9 +368,8 @@ int main(int argc, char *argv[]) towr::TowrUserInterface keyboard_user_interface; - while (ros::ok()) - { - int c = getch(); // call your non-blocking input function + while (ros::ok()) { + int c = getch(); // call your non-blocking input function keyboard_user_interface.CallbackKey(c); refresh(); } @@ -362,4 +378,3 @@ int main(int argc, char *argv[]) return 1; } -