placo 0.7.5__tar.gz → 0.7.6__tar.gz

{placo-0.7.5 → placo-0.7.6}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.4
 Name: placo
-Version: 0.7.5
+Version: 0.7.6
 Summary: PlaCo: Rhoban Planning and Control
 Requires-Python: >= 3.9
 License-Expression: MIT
@@ -16,6 +16,8 @@ Requires-Dist: meshcat
 Requires-Dist: ischedule
 Provides-Extra: build
 Requires-Dist: pin[build] >=  3 ; extra == "build"
+Requires-Dist: cmeel-eigen ; extra == "build"
+Requires-Dist: eigenpy ; extra == "build"
 Requires-Dist: doxystub ; extra == "build"
 Description-Content-Type: text/markdown
 

{placo-0.7.5 → placo-0.7.6}/bindings/expose-robot-wrapper.cpp

@@ -43,6 +43,10 @@ void exposeRobotType(class_<RobotType, W1>& type)
       .def("set_velocity_limits", &RobotType::set_velocity_limits)
       .def("set_torque_limit", &RobotType::set_torque_limit)
       .def("set_joint_limits", &RobotType::set_joint_limits)
+      .def("get_joint_limits", +[](RobotType& robot, const std::string& joint) {
+        auto limits = robot.get_joint_limits(joint);
+        return Eigen::Vector2d(limits.first, limits.second);
+      })
       .def("update_kinematics", &RobotType::update_kinematics)
       .def("compute_hessians", &RobotType::compute_hessians)
       .def(
@@ -203,7 +207,11 @@ void exposeRobotWrapper()
       .def(
           "get_support_side", +[](const HumanoidRobot& robot) { return robot.support_side; })
       .add_property("support_is_both", &HumanoidRobot::support_is_both, &HumanoidRobot::support_is_both)
-      .def_readwrite("T_world_support", &HumanoidRobot::T_world_support);
+      .add_property("support_side", &HumanoidRobot::support_side)
+      .def("get_T_world_support", +[](const HumanoidRobot& robot) { return robot.T_world_support; })
+      .def("set_T_world_support", +[](HumanoidRobot& robot, const Eigen::Affine3d& T_world_support) {
+        robot.T_world_support = T_world_support;
+      });
 
   exposeStdVector<RobotWrapper::Collision>("vector_Collision");
   exposeStdVector<RobotWrapper::Distance>("vector_Distance");

{placo-0.7.5 → placo-0.7.6}/bindings/expose-tools.cpp

@@ -89,14 +89,11 @@ void exposeTools()
   class__<Segment>("Segment", init<Eigen::Vector2d, Eigen::Vector2d>())
       .add_property("start", &Segment::start, &Segment::start)
       .add_property("end", &Segment::end, &Segment::start)
-      .def(
-          "is_parallel", +[](Segment& s1, const Segment& s2) { return s1.is_parallel(s2); })
-      .def(
-          "is_point_aligned", +[](Segment& s, const Eigen::Vector2d& point) { return s.is_point_aligned(point); })
-      .def(
-          "is_collinear", +[](Segment& s1, const Segment& s2) { return s1.is_collinear(s2); })
-      .def(
-          "is_point_in_segment", +[](Segment& s, const Eigen::Vector2d& point) { return s.is_point_in_segment(point); })
+      .def("norm", &Segment::norm)
+      .def("is_parallel", &Segment::is_parallel)
+      .def("is_point_aligned", &Segment::is_point_aligned)
+      .def("is_segment_aligned", &Segment::is_segment_aligned)
+      .def("is_point_in_segment", &Segment::is_point_in_segment)
       .def("intersects", &Segment::intersects)
       .def("line_pass_through", &Segment::line_pass_through)
       .def("half_line_pass_through", &Segment::half_line_pass_through)

{placo-0.7.5 → placo-0.7.6}/placo.pyi

@@ -4133,15 +4133,6 @@ None( (placo.HumanoidParameters)arg1) -> float :
 
 
 class HumanoidRobot:
-  T_world_support: any
-  """
-  
-None( (placo.HumanoidRobot)arg1) -> object :
-
-    C++ signature :
-        Eigen::Transform<double, 3, 2, 0> {lvalue} None(placo::humanoid::HumanoidRobot {lvalue})
-  """
-
   def HumanoidRobot(
     self,
     model_directory: str = "robot", # std::string
@@ -4228,7 +4219,7 @@ None( (placo.HumanoidRobot)arg1) -> object :
     """
     Computes all minimum distances between current collision pairs.
     
-    :return: <Element 'para' at 0x7f79e47dd760>
+    :return: <Element 'para' at 0x7f73dbc4b6a0>
     """
     ...
 
@@ -4250,7 +4241,7 @@ None( (placo.HumanoidRobot)arg1) -> object :
     
     :param any frame: the frame for which we want the jacobian 
     
-    :return: <Element 'para' at 0x7f79e47f44f0>
+    :return: <Element 'para' at 0x7f73dbc49760>
     """
     ...
 
@@ -4264,7 +4255,7 @@ None( (placo.HumanoidRobot)arg1) -> object :
     
     :param any frame: the frame for which we want the jacobian time variation 
     
-    :return: <Element 'para' at 0x7f79e4840540>
+    :return: <Element 'para' at 0x7f73dbbb5490>
     """
     ...
 
@@ -4327,6 +4318,11 @@ None( (placo.HumanoidRobot)arg1) -> object :
   ) -> numpy.ndarray:
     ...
 
+  def get_T_world_support(
+    arg1: HumanoidRobot,
+  ) -> object:
+    ...
+
   def get_T_world_trunk(
     self,
   ) -> numpy.ndarray:
@@ -4378,6 +4374,17 @@ None( (placo.HumanoidRobot)arg1) -> object :
     """
     ...
 
+  def get_joint_limits(
+    self,
+    name: str, # const std::string &
+  ) -> any:
+    """
+    Gets the limits for a given joint.
+    
+    :param str name: joint name
+    """
+    ...
+
   def get_joint_offset(
     self,
     name: str, # const std::string &
@@ -4560,7 +4567,7 @@ None( (placo.HumanoidRobot)arg1) -> object :
     
     :param bool stop_at_first: whether to stop at the first collision found 
     
-    :return: <Element 'para' at 0x7f79e47dff10>
+    :return: <Element 'para' at 0x7f73dbb2be70>
     """
     ...
 
@@ -4589,6 +4596,12 @@ None( (placo.HumanoidRobot)arg1) -> object :
     """
     ...
 
+  def set_T_world_support(
+    arg1: HumanoidRobot,
+    arg2: numpy.ndarray,
+  ) -> None:
+    ...
+
   def set_gear_ratio(
     self,
     joint_name: str, # const std::string &
@@ -4749,6 +4762,15 @@ None( (placo.HumanoidRobot)arg1) -> bool :
         bool {lvalue} None(placo::humanoid::HumanoidRobot {lvalue})
   """
 
+  support_side: any
+  """
+  
+None( (placo.HumanoidRobot)arg1) -> placo.HumanoidRobot_Side :
+
+    C++ signature :
+        placo::humanoid::HumanoidRobot::Side {lvalue} None(placo::humanoid::HumanoidRobot {lvalue})
+  """
+
   def torques_from_acceleration_with_fixed_frame(
     self,
     qdd_a: numpy.ndarray, # Eigen::VectorXd
@@ -7441,7 +7463,7 @@ None( (placo.RobotWrapper)arg1) -> object :
     """
     Computes all minimum distances between current collision pairs.
     
-    :return: <Element 'para' at 0x7f79e48a8950>
+    :return: <Element 'para' at 0x7f73dbcab3d0>
     """
     ...
 
@@ -7455,7 +7477,7 @@ None( (placo.RobotWrapper)arg1) -> object :
     
     :param any frame: the frame for which we want the jacobian 
     
-    :return: <Element 'para' at 0x7f79e48aa930>
+    :return: <Element 'para' at 0x7f73dbc09530>
     """
     ...
 
@@ -7469,7 +7491,7 @@ None( (placo.RobotWrapper)arg1) -> object :
     
     :param any frame: the frame for which we want the jacobian time variation 
     
-    :return: <Element 'para' at 0x7f79e473f560>
+    :return: <Element 'para' at 0x7f73dbdda980>
     """
     ...
 
@@ -7554,6 +7576,17 @@ None( (placo.RobotWrapper)arg1) -> object :
     """
     ...
 
+  def get_joint_limits(
+    self,
+    name: str, # const std::string &
+  ) -> any:
+    """
+    Gets the limits for a given joint.
+    
+    :param str name: joint name
+    """
+    ...
+
   def get_joint_offset(
     self,
     name: str, # const std::string &
@@ -7699,7 +7732,7 @@ None( (placo.RobotWrapper)arg1) -> object :
     
     :param bool stop_at_first: whether to stop at the first collision found 
     
-    :return: <Element 'para' at 0x7f79e48a9210>
+    :return: <Element 'para' at 0x7f73dbcabec0>
     """
     ...
 
@@ -7998,52 +8031,38 @@ None( (placo.Segment)arg1) -> object :
     """
     ...
 
-  def is_collinear(
-    self,
-    s: Segment, # placo::tools::Segment
-    epsilon: float = 1e-6, # double
-  ) -> bool:
-    """
-    Checks if this segment is collinear with another one.
-    
-    :param Segment s: The segment to check collinearity with. 
-    
-    :param float epsilon: To account for floating point errors.
-    """
-    ...
-
   def is_parallel(
     self,
     s: Segment, # placo::tools::Segment
-    epsilon: float = 1e-6, # double
+    epsilon: float = 1e-5, # double
   ) -> bool:
     """
     Checks if this segment is parallel to another one.
     
     :param Segment s: The segment to check parallelism with. 
     
-    :param float epsilon: To account for floating point errors.
+    :param float epsilon: The sinus of the error angle accepted for 2 segments to be considered parallel.
     """
     ...
 
   def is_point_aligned(
     self,
     point: numpy.ndarray, # const Eigen::Vector2d &
-    epsilon: float = 1e-6, # double
+    epsilon: float = 1e-5, # double
   ) -> bool:
     """
-    Checks if a point is aligned with this segment.
+    Checks if a point is aligned with this segment by checking if the segment is parallel to the segment formed by the start of this segment and the point.
     
     :param numpy.ndarray point: The point to check alignment with. 
     
-    :param float epsilon: To account for floating point errors.
+    :param float epsilon: The sinus of the error angle accepted for 2 segments to be considered parallel.
     """
     ...
 
   def is_point_in_segment(
     self,
     point: numpy.ndarray, # const Eigen::Vector2d &
-    epsilon: float = 1e-6, # double
+    epsilon: float = 1e-5, # double
   ) -> bool:
     """
     Checks if a point is in the segment.
@@ -8054,6 +8073,20 @@ None( (placo.Segment)arg1) -> object :
     """
     ...
 
+  def is_segment_aligned(
+    self,
+    s: Segment, # placo::tools::Segment
+    epsilon: float = 1e-5, # double
+  ) -> bool:
+    """
+    Checks if this segment is aligned with another one.
+    
+    :param Segment s: The segment to check alignment with. 
+    
+    :param float epsilon: To account for floating point errors.
+    """
+    ...
+
   def line_pass_through(
     self,
     s: Segment, # placo::tools::Segment
@@ -8076,6 +8109,11 @@ None( (placo.Segment)arg1) -> object :
     """
     ...
 
+  def norm(
+    self,
+  ) -> float:
+    ...
+
   start: any
   """
   

{placo-0.7.5 → placo-0.7.6}/pyproject.toml

@@ -5,6 +5,8 @@ requires = [
   "eiquadprog >= 1.2.6, < 2",
   "pin[build] >=  3",
   "rhoban-cmeel-jsoncpp",
+  "cmeel-eigen",
+  "eigenpy",
   "doxystub"
 ]
 
@@ -22,7 +24,7 @@ description = "PlaCo: Rhoban Planning and Control"
 license = "MIT"
 name = "placo"
 requires-python = ">= 3.9"
-version = "0.7.5"
+version = "0.7.6"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"

{placo-0.7.5 → placo-0.7.6}/src/placo/tools/segment.cpp

@@ -12,11 +12,16 @@ Segment::Segment(const Eigen::Vector2d& start, const Eigen::Vector2d& end)
 {
 }
 
+double Segment::norm()
+{
+    return (end - start).norm();
+}
+
 bool Segment::is_parallel(const Segment& s, double epsilon)
 {
     Eigen::Vector2d v1 = end - start;
     Eigen::Vector2d v2 = s.end - s.start;
-    return abs(v1.x() * v2.y() - v1.y() * v2.x()) < epsilon;
+    return abs(v1.x() * v2.y() - v1.y() * v2.x()) / (v1.norm() * v2.norm()) < epsilon;
 }
 
 bool Segment::is_point_aligned(const Eigen::Vector2d& point, double epsilon)
@@ -25,12 +30,10 @@ bool Segment::is_point_aligned(const Eigen::Vector2d& point, double epsilon)
     {
         return true;
     }
-    double alpha_x = (point.x() - start.x()) / (end.x() - start.x());
-    double alpha_y = (point.y() - start.y()) / (end.y() - start.y());
-    return abs(alpha_x - alpha_y) < epsilon;
+    return is_parallel(Segment(start, point), epsilon);
 }
 
-bool Segment::is_collinear(const Segment& s, double epsilon)
+bool Segment::is_segment_aligned(const Segment& s, double epsilon)
 {
     return is_point_aligned(s.start, epsilon) && is_point_aligned(s.end, epsilon);
 }
@@ -39,7 +42,7 @@ bool Segment::is_point_in_segment(const Eigen::Vector2d& point, double epsilon)
 {
     Eigen::Vector2d v1 = end - start;
     Eigen::Vector2d v2 = point - start;
-    return is_collinear(Segment(start, point), epsilon) && v1.dot(v2) >= 0 && v1.dot(v2) <= v1.dot(v1);
+    return is_segment_aligned(Segment(start, point), epsilon) && v1.dot(v2) >= 0 && v1.dot(v2) <= v1.dot(v1);
 }
 
 bool Segment::intersects(Segment& s)
@@ -51,7 +54,7 @@ bool Segment::intersects(Segment& s)
     return line_pass_through(s) && s.line_pass_through(*this);
 }
 
-bool Segment::line_pass_through(const Segment& s)
+std::pair<double, double> Segment::get_lambdas(const Segment& s)
 {
     if (is_parallel(s))
     {
@@ -63,41 +66,27 @@ bool Segment::line_pass_through(const Segment& s)
     Eigen::Vector2d p1 = start;
     Eigen::Vector2d p2 = s.start;
     double det = v1.x() * v2.y() - v1.y() * v2.x();
-    double t = (v2.y() * (p2.x() - p1.x()) + v2.x() * (p1.y() - p2.y())) / det;
-    return t >= 0 && t <= 1;
+    double l1 = (v2.y() * (p2.x() - p1.x()) + v2.x() * (p1.y() - p2.y())) / det;
+    double l2 = (v1.y() * (p2.x() - p1.x()) + v1.x() * (p1.y() - p2.y())) / det;
+    return std::make_pair(l1, l2);
 }
 
-bool Segment::half_line_pass_through(const Segment& s)
+bool Segment::line_pass_through(const Segment& s)
 {
-    if (is_parallel(s))
-    {
-        throw std::runtime_error("Can't compute intersection of parallels");
-    }
+    auto lambdas = get_lambdas(s);
+    return lambdas.first >= 0 && lambdas.first <= 1;
+}
 
-    Eigen::Vector2d v1 = end - start;
-    Eigen::Vector2d v2 = s.end - s.start;
-    Eigen::Vector2d p1 = start;
-    Eigen::Vector2d p2 = s.start;
-    double det = v1.x() * v2.y() - v1.y() * v2.x();
-    double t_1 = (v2.y() * (p2.x() - p1.x()) + v2.x() * (p1.y() - p2.y())) / det;
-    double t_2 = (v1.y() * (p2.x() - p1.x()) + v1.x() * (p1.y() - p2.y())) / det;
-    return t_1 >= 0 && t_1 <= 1 && t_2 >= 0;
+bool Segment::half_line_pass_through(const Segment& s)
+{
+    auto lambdas = get_lambdas(s);
+    return lambdas.first >= 0 && lambdas.first <= 1 && lambdas.second >= 0;
 }
 
 Eigen::Vector2d Segment::lines_intersection(const Segment& s)
 {
-    if (is_parallel(s))
-    {
-        throw std::runtime_error("Can't compute intersection of parallels");
-    }
-
-    Eigen::Vector2d v1 = end - start;
-    Eigen::Vector2d v2 = s.end - s.start;
-    Eigen::Vector2d p1 = start;
-    Eigen::Vector2d p2 = s.start;
-    double det = v1.x() * v2.y() - v1.y() * v2.x();
-    double t = (v2.y() * (p2.x() - p1.x()) + v2.x() * (p1.y() - p2.y())) / det;
-    return p1 + t * v1;
+    auto lambdas = get_lambdas(s);
+    Eigen::Vector2d v = end - start;
+    return start + lambdas.first * v;
 }
 }  // namespace placo::tools
-

{placo-0.7.5 → placo-0.7.6}/src/placo/tools/segment.h

@@ -12,30 +12,32 @@ public:
 
     Eigen::Vector2d start;
     Eigen::Vector2d end;
-
+    double norm();
+    
     /**
      * @brief Checks if this segment is parallel to another one.
      * @param s The segment to check parallelism with.
-     * @param epsilon To account for floating point errors.
+     * @param epsilon The sinus of the error angle accepted for 2 segments to be considered parallel.
      * @return True if the segments are parallel.
      */
-    bool is_parallel(const Segment& s, double epsilon = 1e-6);
+    bool is_parallel(const Segment& s, double epsilon = 1e-5);
 
     /**
-     * @brief Checks if a point is aligned with this segment.
+     * @brief Checks if a point is aligned with this segment by checking if the segment
+     * is parallel to the segment formed by the start of this segment and the point.
      * @param point The point to check alignment with.
-     * @param epsilon To account for floating point errors.
+     * @param epsilon The sinus of the error angle accepted for 2 segments to be considered parallel.
      * @return True if the point is aligned with the segment.
      */
-    bool is_point_aligned(const Eigen::Vector2d& point, double epsilon = 1e-6);
+    bool is_point_aligned(const Eigen::Vector2d& point, double epsilon = 1e-5);
 
     /**
-     * @brief Checks if this segment is collinear with another one.
-     * @param s The segment to check collinearity with.
+     * @brief Checks if this segment is aligned with another one.
+     * @param s The segment to check alignment with.
      * @param epsilon To account for floating point errors.
      * @return True if the segments are collinear.
      */
-    bool is_collinear(const Segment& s, double epsilon = 1e-6);
+    bool is_segment_aligned(const Segment& s, double epsilon = 1e-5);
 
     /**
      * @brief Checks if a point is in the segment.
@@ -43,7 +45,7 @@ public:
      * @param epsilon To account for floating point errors.
      * @return True if the point is in the segment.
      */
-    bool is_point_in_segment(const Eigen::Vector2d& point, double epsilon = 1e-6);
+    bool is_point_in_segment(const Eigen::Vector2d& point, double epsilon = 1e-5);
 
     /**
      * @brief Checks if there is an intersection between this segment and another one, i.e. if the 
@@ -53,6 +55,13 @@ public:
      */
     bool intersects(Segment& s);
 
+    /**
+     * @brief Returns the lambda values of the intersection between this segment and another one.
+     * @param s The other segment.
+     * @return A pair of doubles containing the lambda values of the intersection.
+     */
+    std::pair<double, double> get_lambdas(const Segment& s);
+
     /**
      * @brief Checks if the guiding line of another segment pass through this segment, i.e. if the 
      * intersection between the guiding lines of this segment and another one is a point of this segment.

{placo-0.7.5 → placo-0.7.6}/CMakeLists.txt

@@ -1,5 +1,5 @@
-project(placo)
 cmake_minimum_required(VERSION 3.10)
+project(placo)
 
 find_package(Threads)
 find_package(pinocchio REQUIRED)