update docs and tests

CMakeLists.txt

@@ -34,13 +34,14 @@ add_library(
   src/cost/cost_function_data.cpp
   src/cost/cost_function_component_base.cpp
   src/cost/configuration_space_cost.cpp
-  src/cost/time_varying_configuration_space_cost.cpp
-  src/cost/contact_force_cost.cpp
   src/cost/task_space_3d_cost.cpp
   src/cost/task_space_6d_cost.cpp
+  src/cost/com_cost.cpp
+  src/cost/contact_force_cost.cpp
+  src/cost/time_varying_configuration_space_cost.cpp
   src/cost/time_varying_task_space_3d_cost.cpp
   src/cost/time_varying_task_space_6d_cost.cpp
-  src/cost/trotting_configuration_space_cost.cpp
+  src/cost/time_varying_com_cost.cpp
   src/constraints/pdipm.cpp
   src/constraints/constraints.cpp
   src/constraints/constraints_impl.cpp

LICENSE

@@ -1,6 +1,6 @@
 BSD 3-Clause License
 
-Copyright (c) 2020, Sotaro Katayama, Toshiyuki Ohtsuka.
+Copyright (c) 2020-2021, Sotaro Katayama, Toshiyuki Ohtsuka.
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

examples/iiwa14/task_space_ocp.cpp

@@ -67,8 +67,8 @@ int main(int argc, char *argv[]) {
   const int ee_frame_id = 22; 
   auto ref = std::make_shared<TimeVaryingTaskSpace6DRef>();
   auto task_cost = std::make_shared<idocp::TimeVaryingTaskSpace6DCost>(robot, ee_frame_id, ref);
-  task_cost->set_q_6d_weight(Eigen::Vector3d::Constant(1000), Eigen::Vector3d::Constant(1000));
-  task_cost->set_qf_6d_weight(Eigen::Vector3d::Constant(1000), Eigen::Vector3d::Constant(1000));
+  task_cost->set_q_weight(Eigen::Vector3d::Constant(1000), Eigen::Vector3d::Constant(1000));
+  task_cost->set_qf_weight(Eigen::Vector3d::Constant(1000), Eigen::Vector3d::Constant(1000));
   cost->push_back(task_cost);
 
   // Create joint constraints.

include/idocp/constraints/constraints_impl.hpp

@@ -222,7 +222,6 @@ double costSlackBarrier(
 /// @param[in] constraints Vector of the impulse constraints. 
 /// @param[in] robot Robot model.
 /// @param[in, out] data Vector of the constraints data.
-/// @param[in] data Constraint data.
 /// @param[in] s Split solution.
 ///
 template <typename ConstraintComponentBaseTypePtr, typename SplitSolutionType>

include/idocp/constraints/friction_cone.hpp

@@ -63,13 +63,17 @@ class FrictionCone final : public ConstraintComponentBase {
   ///
   FrictionCone& operator=(FrictionCone&&) noexcept = default;
 
+  ///
+  /// @brief Sets the friction coefficient. 
+  /// @param[in] mu Friction coefficient. Must be positive.
+  ///
   void setFrictionCoefficient(const double mu);
 
   bool useKinematics() const override;
 
   KinematicsLevel kinematicsLevel() const override;
 
-  void allocateExtraData(ConstraintComponentData& data) const;
+  void allocateExtraData(ConstraintComponentData& data) const override;
 
   bool isFeasible(Robot& robot, ConstraintComponentData& data, 
                   const SplitSolution& s) const override;

include/idocp/constraints/impulse_friction_cone.hpp

@@ -63,11 +63,15 @@ class ImpulseFrictionCone final : public ImpulseConstraintComponentBase {
   ///
   ImpulseFrictionCone& operator=(ImpulseFrictionCone&&) noexcept = default;
 
+  ///
+  /// @brief Sets the friction coefficient. 
+  /// @param[in] mu Friction coefficient. Must be positive.
+  ///
   void setFrictionCoefficient(const double mu);
 
   KinematicsLevel kinematicsLevel() const override;
 
-  void allocateExtraData(ConstraintComponentData& data) const;
+  void allocateExtraData(ConstraintComponentData& data) const override;
 
   bool isFeasible(Robot& robot, ConstraintComponentData& data, 
                   const ImpulseSplitSolution& s) const override;

include/idocp/cost/com_cost.hpp

@@ -0,0 +1,136 @@
+#ifndef IDOCP_COM_COST_HPP_
+#define IDOCP_COM_COST_HPP_
+
+#include "Eigen/Core"
+
+#include "idocp/robot/robot.hpp"
+#include "idocp/cost/cost_function_component_base.hpp"
+#include "idocp/cost/cost_function_data.hpp"
+#include "idocp/ocp/split_solution.hpp"
+#include "idocp/ocp/split_kkt_residual.hpp"
+#include "idocp/ocp/split_kkt_matrix.hpp"
+#include "idocp/impulse/impulse_split_solution.hpp"
+#include "idocp/impulse/impulse_split_kkt_residual.hpp"
+#include "idocp/impulse/impulse_split_kkt_matrix.hpp"
+
+
+namespace idocp {
+
+///
+/// @class CoMCost
+/// @brief Cost on the position of the center of mass. 
+///
+class CoMCost final : public CostFunctionComponentBase {
+public:
+  ///
+  /// @brief Constructor. 
+  /// @param[in] robot Robot model.
+  ///
+  CoMCost(const Robot& robot);
+
+  ///
+  /// @brief Default constructor. 
+  ///
+  CoMCost();
+
+  ///
+  /// @brief Destructor. 
+  ///
+  ~CoMCost();
+
+  ///
+  /// @brief Default copy constructor. 
+  ///
+  CoMCost(const CoMCost&) = default;
+
+  ///
+  /// @brief Default copy operator. 
+  ///
+  CoMCost& operator=(const CoMCost&) = default;
+
+  ///
+  /// @brief Default move constructor. 
+  ///
+  CoMCost(CoMCost&&) noexcept = default;
+
+  ///
+  /// @brief Default move assign operator. 
+  ///
+  CoMCost& operator=(CoMCost&&) noexcept = default;
+
+  ///
+  /// @brief Sets the reference position of the center of mass. 
+  /// @param[in] CoM_ref Reference position of the center of mass.
+  ///
+  void set_CoM_ref(const Eigen::Vector3d& CoM_ref);
+
+  ///
+  /// @brief Sets the weight vector. 
+  /// @param[in] q_weight Weight vector on the CoM position error. 
+  ///
+  void set_q_weight(const Eigen::Vector3d& q_weight);
+
+  ///
+  /// @brief Sets the terminal weight vector. 
+  /// @param[in] qf_weight Terminal weight vector on the CoM position error. 
+  ///
+  void set_qf_weight(const Eigen::Vector3d& qf_weight);
+
+  ///
+  /// @brief Sets the weight vector at impulse. 
+  /// @param[in] qi_weight Weight vector on the CoM position error at impulse. 
+  ///
+  void set_qi_weight(const Eigen::Vector3d& qi_weight);
+
+  bool useKinematics() const override;
+
+  double computeStageCost(Robot& robot, CostFunctionData& data, const double t, 
+                          const double dt, 
+                          const SplitSolution& s) const override;
+
+  double computeTerminalCost(Robot& robot, CostFunctionData& data, 
+                             const double t, 
+                             const SplitSolution& s) const override;
+
+  double computeImpulseCost(Robot& robot, CostFunctionData& data, 
+                            const double t, 
+                            const ImpulseSplitSolution& s) const override;
+
+  void computeStageCostDerivatives(
+      Robot& robot, CostFunctionData& data, const double t, const double dt, 
+      const SplitSolution& s, SplitKKTResidual& kkt_residual) const override;
+
+  void computeTerminalCostDerivatives(
+      Robot& robot, CostFunctionData& data, const double t, 
+      const SplitSolution& s, SplitKKTResidual& kkt_residual) const override;
+
+  void computeImpulseCostDerivatives(
+      Robot& robot, CostFunctionData& data, const double t, 
+      const ImpulseSplitSolution& s, 
+      ImpulseSplitKKTResidual& kkt_residual) const;
+
+  void computeStageCostHessian(Robot& robot, CostFunctionData& data, 
+                               const double t, const double dt, 
+                               const SplitSolution& s, 
+                               SplitKKTMatrix& kkt_matrix) const override;
+
+  void computeTerminalCostHessian(Robot& robot, CostFunctionData& data, 
+                                  const double t, const SplitSolution& s, 
+                                  SplitKKTMatrix& kkt_matrix) const override;
+
+  void computeImpulseCostHessian(
+      Robot& robot, CostFunctionData& data, const double t, 
+      const ImpulseSplitSolution& s, 
+      ImpulseSplitKKTMatrix& kkt_matrix) const override;
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+private:
+  Eigen::Vector3d CoM_ref_, q_weight_, qf_weight_, qi_weight_;
+
+};
+
+} // namespace idocp
+
+
+#endif // IDOCP_COM_COST_HPP_ 

include/idocp/cost/configuration_space_cost.hpp

@@ -18,7 +18,7 @@ namespace idocp {
 
 ///
 /// @class ConfigurationSpaceCost
-/// @brief Configuration space quadratic cost. 
+/// @brief Configuration space cost. 
 ///
 class ConfigurationSpaceCost final : public CostFunctionComponentBase {
 public:
@@ -68,13 +68,13 @@ class ConfigurationSpaceCost final : public CostFunctionComponentBase {
 
   ///
   /// @brief Sets the reference velocity v. 
-  /// @param[in] q_ref Reference velocity v. Size must be Robot::dimv().
+  /// @param[in] v_ref Reference velocity v. Size must be Robot::dimv().
   ///
   void set_v_ref(const Eigen::VectorXd& v_ref);
 
   ///
   /// @brief Sets the reference control input torques u. 
-  /// @param[in] q_ref Reference control input torques u. Size must be 
+  /// @param[in] u_ref Reference control input torques u. Size must be 
   /// Robot::dimu().
   ///
   void set_u_ref(const Eigen::VectorXd& u_ref);

include/idocp/cost/contact_force_cost.hpp

@@ -17,7 +17,7 @@ namespace idocp {
 
 ///
 /// @class ContactForceCost
-/// @brief Quadratic cost on the contact forces expressed in the local frames.
+/// @brief Cost on the contact forces expressed in the local frames.
 ///
 class ContactForceCost final : public CostFunctionComponentBase {
 public:
@@ -73,7 +73,7 @@ class ContactForceCost final : public CostFunctionComponentBase {
 
   ///
   /// @brief Sets the reference impulse forces expressed in the local frames. 
-  /// @param[in] f_ref Reference impulse forces expressed in the local frames. 
+  /// @param[in] fi_ref Reference impulse forces expressed in the local frames. 
   /// Size must be Robot::maxPointContacts().
   ///
   void set_fi_ref(const std::vector<Eigen::Vector3d>& fi_ref);

include/idocp/cost/cost_function_data.hxx

@@ -6,7 +6,7 @@
 namespace idocp {
 
 inline CostFunctionData::CostFunctionData(const Robot& robot) 
-  : qdiff(),
+  : qdiff(Eigen::VectorXd::Zero(robot.dimv())),
     q_ref(Eigen::VectorXd::Zero(robot.dimq())),
     q_3d_ref(Eigen::VectorXd::Zero(3)),
     diff_3d(Eigen::VectorXd::Zero(3)),

include/idocp/cost/task_space_3d_cost.hpp

@@ -18,7 +18,7 @@ namespace idocp {
 
 ///
 /// @class TaskSpace3DCost
-/// @brief Quadratic cost on the task space position. 
+/// @brief Cost on the task space position. 
 ///
 class TaskSpace3DCost final : public CostFunctionComponentBase {
 public:
@@ -69,19 +69,19 @@ class TaskSpace3DCost final : public CostFunctionComponentBase {
   /// @brief Sets the weight vector. 
   /// @param[in] q_3d_weight Weight vector on the position error. 
   ///
-  void set_q_3d_weight(const Eigen::Vector3d& q_3d_weight);
+  void set_q_weight(const Eigen::Vector3d& q_3d_weight);
 
   ///
   /// @brief Sets the terminal weight vector. 
-  /// @param[in] q_3d_weight Terminal weight vector on the position error. 
+  /// @param[in] qf_3d_weight Terminal weight vector on the position error. 
   ///
-  void set_qf_3d_weight(const Eigen::Vector3d& qf_3d_weight);
+  void set_qf_weight(const Eigen::Vector3d& qf_3d_weight);
 
   ///
   /// @brief Sets the weight vector at impulse. 
-  /// @param[in] q_3d_weight Weight vector on the position error at impulse. 
+  /// @param[in] qi_3d_weight Weight vector on the position error at impulse. 
   ///
-  void set_qi_3d_weight(const Eigen::Vector3d& qi_3d_weight);
+  void set_qi_weight(const Eigen::Vector3d& qi_3d_weight);
 
   bool useKinematics() const override;
 

include/idocp/cost/task_space_6d_cost.hpp

@@ -19,7 +19,7 @@ namespace idocp {
 
 ///
 /// @class TaskSpace6DCost
-/// @brief Quadratic cost on the task space pose (SE(3). 
+/// @brief Cost on the task space placement (SE(3)). 
 ///
 class TaskSpace6DCost final : public CostFunctionComponentBase {
 public:
@@ -75,23 +75,23 @@ class TaskSpace6DCost final : public CostFunctionComponentBase {
   /// @param[in] position_weight Weight vector on the position error. 
   /// @param[in] rotation_weight Weight vector on the rotation error. 
   ///
-  void set_q_6d_weight(const Eigen::Vector3d& position_weight, 
+  void set_q_weight(const Eigen::Vector3d& position_weight, 
                        const Eigen::Vector3d& rotation_weight);
 
   ///
   /// @brief Sets the terminal weight vectors. 
   /// @param[in] position_weight Temrinal weight vector on the position error. 
   /// @param[in] rotation_weight Temrinal weight vector on the rotation error. 
   ///
-  void set_qf_6d_weight(const Eigen::Vector3d& position_weight, 
+  void set_qf_weight(const Eigen::Vector3d& position_weight, 
                         const Eigen::Vector3d& rotation_weight);
 
   ///
   /// @brief Sets the weight vectors at impulse. 
   /// @param[in] position_weight Weight vector on the position error at impulse. 
   /// @param[in] rotation_weight Weight vector on the rotation error at impulse. 
   ///
-  void set_qi_6d_weight(const Eigen::Vector3d& position_weight, 
+  void set_qi_weight(const Eigen::Vector3d& position_weight, 
                         const Eigen::Vector3d& rotation_weight);
 
   bool useKinematics() const override;