placo 0.8.10__tar.gz → 0.9.0__tar.gz

{placo-0.8.10 → placo-0.9.0}/CMakeLists.txt

@@ -89,6 +89,8 @@ add_library(libplaco SHARED
     src/placo/kinematics/com_polygon_constraint.cpp
     src/placo/kinematics/joint_space_half_spaces_constraint.cpp
     src/placo/kinematics/cone_constraint.cpp
+    src/placo/kinematics/yaw_constraint.cpp
+    src/placo/kinematics/distance_constraint.cpp
     src/placo/kinematics/axis_align_task.cpp
 
     # Dynamics QP solver

{placo-0.8.10 → placo-0.9.0}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: placo
-Version: 0.8.10
+Version: 0.9.0
 Summary: PlaCo: Rhoban Planning and Control
 Requires-Python: >= 3.9
 License-Expression: MIT

{placo-0.8.10 → placo-0.9.0}/bindings/expose-kinematics.cpp

@@ -101,6 +101,14 @@ void exposeKinematics()
           .def<ConeConstraint& (KinematicsSolver::*)(std::string, std::string, double)>(
               "add_cone_constraint", &KinematicsSolver::add_cone_constraint, return_internal_reference<>())
 
+          // Yaw constraint
+          .def<YawConstraint& (KinematicsSolver::*)(std::string, std::string, double)>(
+              "add_yaw_constraint", &KinematicsSolver::add_yaw_constraint, return_internal_reference<>())
+
+          // Distance constraint
+          .def<DistanceConstraint& (KinematicsSolver::*)(std::string, std::string, double)>(
+              "add_distance_constraint", &KinematicsSolver::add_distance_constraint, return_internal_reference<>())
+
           // Masking/unmasking DoFs
           .def("mask_dof", &KinematicsSolver::mask_dof)
           .def("unmask_dof", &KinematicsSolver::unmask_dof)
@@ -246,4 +254,12 @@ void exposeKinematics()
       .def_readwrite("angle_max", &ConeConstraint::angle_max)
       .def_readwrite("N", &ConeConstraint::N)
       .def_readwrite("range", &ConeConstraint::range);
+
+  class__<YawConstraint, bases<Constraint>>(
+      "YawConstraint", init<model::RobotWrapper::FrameIndex, model::RobotWrapper::FrameIndex, double>())
+      .def_readwrite("angle_max", &YawConstraint::angle_max);
+
+  class__<DistanceConstraint, bases<Constraint>>(
+      "DistanceConstraint", init<model::RobotWrapper::FrameIndex, model::RobotWrapper::FrameIndex, double>())
+      .def_readwrite("distance_max", &DistanceConstraint::distance_max);
 }

{placo-0.8.10 → placo-0.9.0}/pyproject.toml

@@ -23,7 +23,7 @@ description = "PlaCo: Rhoban Planning and Control"
 license = "MIT"
 name = "placo"
 requires-python = ">= 3.9"
-version = "0.8.10"
+version = "0.9.0"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"

placo-0.9.0/src/placo/kinematics/distance_constraint.cpp

@@ -0,0 +1,41 @@
+#include "placo/kinematics/distance_constraint.h"
+#include "placo/kinematics/kinematics_solver.h"
+#include "placo/problem/polygon_constraint.h"
+
+namespace placo::kinematics
+{
+DistanceConstraint::DistanceConstraint(model::RobotWrapper::FrameIndex frame_a, model::RobotWrapper::FrameIndex frame_b,
+                                       double distance_max)
+  : frame_a(frame_a), frame_b(frame_b), distance_max(distance_max)
+{
+}
+
+void DistanceConstraint::add_constraint(placo::problem::Problem& problem)
+{
+  auto T_world_a = solver->robot.get_T_world_frame(frame_a);
+  auto T_world_b = solver->robot.get_T_world_frame(frame_b);
+
+  Eigen::Vector3d ab_world = T_world_b.translation() - T_world_a.translation();
+
+  double distance = ab_world.norm();
+  Eigen::Vector3d direction = ab_world.normalized();
+
+  Eigen::MatrixXd J_a = solver->robot.frame_jacobian(frame_a, pinocchio::LOCAL_WORLD_ALIGNED);
+  Eigen::MatrixXd J_b = solver->robot.frame_jacobian(frame_b, pinocchio::LOCAL_WORLD_ALIGNED);
+  Eigen::MatrixXd J_distance = direction.transpose() * (J_b - J_a).block(0, 0, 3, solver->N);
+
+  // Expression for the angle
+  problem::Expression e;
+  e.A.resize(1, solver->N);
+  e.b.resize(1);
+
+  e.A.row(0) = J_distance;
+  e.b(0) = distance;
+
+  problem.add_constraint(e <= distance_max)
+      .configure(priority == Prioritized::Priority::Hard ? problem::ProblemConstraint::Hard :
+                                                           problem::ProblemConstraint::Soft,
+                 weight);
+}
+
+}  // namespace placo::kinematics

placo-0.9.0/src/placo/kinematics/distance_constraint.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <vector>
+#include "placo/problem/problem.h"
+#include "placo/kinematics/constraint.h"
+
+namespace placo::kinematics
+{
+class KinematicsSolver;
+
+/**
+ * @brief Constraints the distance betweek two points in the robot
+ */
+class DistanceConstraint : public Constraint
+{
+public:
+  /**
+   * @brief Constraints the distance betweek two points in the robot
+   * @param frame_a
+   * @param frame_b
+   * @param distance_max
+   */
+  DistanceConstraint(model::RobotWrapper::FrameIndex frame_a, model::RobotWrapper::FrameIndex frame_b,
+                     double distance_max);
+
+  /**
+   * @brief Frame A
+   */
+  model::RobotWrapper::FrameIndex frame_a;
+
+  /**
+   * @brief Frame B
+   */
+  model::RobotWrapper::FrameIndex frame_b;
+
+  /**
+   * @brief Maximum distance allowed between frame A and frame B
+   */
+  double distance_max;
+
+  virtual void add_constraint(placo::problem::Problem& problem) override;
+};
+}  // namespace placo::kinematics

{placo-0.8.10 → placo-0.9.0}/src/placo/kinematics/kinematics_solver.cpp

@@ -226,6 +226,30 @@ ConeConstraint& KinematicsSolver::add_cone_constraint(std::string frame_a, std::
   return add_cone_constraint(robot.get_frame_index(frame_a), robot.get_frame_index(frame_b), angle_max);
 }
 
+YawConstraint& KinematicsSolver::add_yaw_constraint(model::RobotWrapper::FrameIndex frame_a,
+                                                    model::RobotWrapper::FrameIndex frame_b, double angle_max)
+{
+  return add_constraint(new YawConstraint(frame_a, frame_b, angle_max));
+}
+
+YawConstraint& KinematicsSolver::add_yaw_constraint(std::string frame_a, std::string frame_b, double angle_max)
+{
+  return add_yaw_constraint(robot.get_frame_index(frame_a), robot.get_frame_index(frame_b), angle_max);
+}
+
+DistanceConstraint& KinematicsSolver::add_distance_constraint(model::RobotWrapper::FrameIndex frame_a,
+                                                              model::RobotWrapper::FrameIndex frame_b,
+                                                              double distance_max)
+{
+  return add_constraint(new DistanceConstraint(frame_a, frame_b, distance_max));
+}
+
+DistanceConstraint& KinematicsSolver::add_distance_constraint(std::string frame_a, std::string frame_b,
+                                                              double distance_max)
+{
+  return add_distance_constraint(robot.get_frame_index(frame_a), robot.get_frame_index(frame_b), distance_max);
+}
+
 void KinematicsSolver::mask_dof(std::string dof)
 {
   masked_dof.insert(robot.get_joint_v_offset(dof));

{placo-0.8.10 → placo-0.9.0}/src/placo/kinematics/kinematics_solver.h

@@ -30,6 +30,8 @@
 #include "placo/kinematics/com_polygon_constraint.h"
 #include "placo/kinematics/joint_space_half_spaces_constraint.h"
 #include "placo/kinematics/cone_constraint.h"
+#include "placo/kinematics/yaw_constraint.h"
+#include "placo/kinematics/distance_constraint.h"
 
 // Problem formulation
 #include "placo/problem/problem.h"
@@ -300,7 +302,6 @@ public:
    * @param frame_a frame A
    * @param frame_b frame B
    * @param alpha_max alpha max (in radians) between the frame z-axis and the cone frame zt-axis
-   * @param T_world_cone cone frame
    * @return constraint
    * @pyignore
    */
@@ -312,11 +313,50 @@ public:
    * @param frame_a frame A
    * @param frame_b frame B
    * @param alpha_max alpha max (in radians) between the frame z-axis and the cone frame zt-axis
-   * @param T_world_cone cone frame
    * @return constraint
    */
   ConeConstraint& add_cone_constraint(std::string frame_a, std::string frame_b, double alpha_max);
 
+  /**
+   * @brief Adds a yaw constraint
+   * @param frame_a frame A
+   * @param frame_b frame B
+   * @param alpha_max angle max for yaw of x-axis in frame b in a
+   * @return constraint
+   * @pyignore
+   */
+  YawConstraint& add_yaw_constraint(model::RobotWrapper::FrameIndex frame_a, model::RobotWrapper::FrameIndex frame_b,
+                                    double alpha_max);
+
+  /**
+   * @brief Adds a yaw constraint
+   * @param frame_a frame A
+   * @param frame_b frame B
+   * @param alpha_max angle max for yaw of x-axis in frame b in a
+   * @return constraint
+   */
+  YawConstraint& add_yaw_constraint(std::string frame_a, std::string frame_b, double alpha_max);
+
+  /**
+   * @brief Adds a distance constraint
+   * @param frame_a frame A
+   * @param frame_b frame B
+   * @param distance_max maximum distance between the two frames
+   * @return constraint
+   * @pyignore
+   */
+  DistanceConstraint& add_distance_constraint(model::RobotWrapper::FrameIndex frame_a,
+                                              model::RobotWrapper::FrameIndex frame_b, double distance_max);
+
+  /**
+   * @brief Adds a distance constraint
+   * @param frame_a frame A
+   * @param frame_b frame B
+   * @param distance_max maximum distance between the two frames
+   * @return constraint
+   */
+  DistanceConstraint& add_distance_constraint(std::string frame_a, std::string frame_b, double distance_max);
+
   /**
    * @brief Constructs the QP problem and solves it
    * @param apply apply the solution to the robot model

placo-0.9.0/src/placo/kinematics/yaw_constraint.cpp

@@ -0,0 +1,52 @@
+#include "placo/kinematics/yaw_constraint.h"
+#include "placo/kinematics/kinematics_solver.h"
+#include "placo/problem/polygon_constraint.h"
+
+namespace placo::kinematics
+{
+YawConstraint::YawConstraint(model::RobotWrapper::FrameIndex frame_a, model::RobotWrapper::FrameIndex frame_b,
+                             double angle_max)
+  : frame_a(frame_a), frame_b(frame_b), angle_max(angle_max)
+{
+}
+
+void YawConstraint::add_constraint(placo::problem::Problem& problem)
+{
+  Eigen::Affine3d T_a_b = solver->robot.get_T_a_b(frame_a, frame_b);
+
+  // Jacobian of the rotational velocity expressed in a
+  Eigen::MatrixXd J_relative =
+      T_a_b.linear() *
+          solver->robot.frame_jacobian(frame_b, pinocchio::ReferenceFrame::LOCAL).block(3, 0, 3, solver->N) -
+      solver->robot.frame_jacobian(frame_a, pinocchio::ReferenceFrame::LOCAL).block(3, 0, 3, solver->N);
+  ;
+
+  // B's x axis in A
+  Eigen::Vector3d x_axis = T_a_b.linear().block(0, 0, 3, 1);
+
+  // Current angle
+  double alpha = atan2(x_axis.y(), x_axis.x());
+
+  Eigen::Vector3d perp_axis = Eigen::Vector3d::UnitZ().cross(x_axis).normalized();
+  Eigen::Vector3d yaw_axis = x_axis.cross(perp_axis).normalized();
+
+  Eigen::MatrixXd J_angle = yaw_axis.transpose() * J_relative;
+
+  // Expression for the angle
+  problem::Expression e;
+  e.A.resize(2, solver->N);
+  e.b.resize(2);
+  // First row is angle + J_angle*qd
+  e.A.block(0, 0, 1, solver->N) = J_angle;
+  e.b(0, 0) = alpha;
+  // Second row is -(angle -J_angle*qd)
+  e.A.block(1, 0, 1, solver->N) = -J_angle;
+  e.b(1, 0) = -alpha;
+
+  problem.add_constraint(e <= angle_max)
+      .configure(priority == Prioritized::Priority::Hard ? problem::ProblemConstraint::Hard :
+                                                           problem::ProblemConstraint::Soft,
+                 weight);
+}
+
+}  // namespace placo::kinematics

placo-0.9.0/src/placo/kinematics/yaw_constraint.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include <vector>
+#include "placo/problem/problem.h"
+#include "placo/kinematics/constraint.h"
+
+namespace placo::kinematics
+{
+class KinematicsSolver;
+
+/**
+ * @brief A yaw constraint is a constraint where the x-axis of frame b should have a yaw withing +- angle_max in frame a
+ */
+class YawConstraint : public Constraint
+{
+public:
+  /**
+   * @brief Constrains the yaw of frame b in frame a, such that the x-axis of frame b should remain with +- angle_max
+   * @param frame_a
+   * @param frame_b
+   * @param angle_max
+   */
+  YawConstraint(model::RobotWrapper::FrameIndex frame_a, model::RobotWrapper::FrameIndex frame_b, double angle_max);
+
+  /**
+   * @brief Frame A
+   */
+  model::RobotWrapper::FrameIndex frame_a;
+
+  /**
+   * @brief Frame B
+   */
+  model::RobotWrapper::FrameIndex frame_b;
+
+  /**
+   * @brief Maximum angle allowable by the yaw constraint (yaw is taken for x-axis of b around z-axis in a)
+   */
+  double angle_max;
+
+  virtual void add_constraint(placo::problem::Problem& problem) override;
+};
+}  // namespace placo::kinematics