placo 0.3.6__tar.gz → 0.3.8__tar.gz

{placo-0.3.6 → placo-0.3.8}/PKG-INFO

@@ -1,6 +1,6 @@
 Metadata-Version: 2.1
 Name: placo
-Version: 0.3.6
+Version: 0.3.8
 Summary: PlaCo: Rhoban Planning and Control
 Requires-Python: >= 3.8
 License-Expression: MIT
@@ -14,6 +14,7 @@ Requires-Dist: pin >= 2.6.18, < 3
 Requires-Dist: rhoban-cmeel-jsoncpp
 Requires-Dist: meshcat
 Requires-Dist: numpy
+Requires-Dist: ischedule
 Provides-Extra: build
 Requires-Dist: pin[build] >= 2.6.18, < 3 ; extra == "build"
 Requires-Dist: cmeel-urdfdom[build] ; extra == "build"

{placo-0.3.6 → placo-0.3.8}/bindings/expose-robot-wrapper.cpp

@@ -33,6 +33,8 @@ void exposeRobotType(class_<RobotType, W1>& type)
       .def("get_joint", &RobotType::get_joint)
       .def("set_joint_velocity", &RobotType::set_joint_velocity)
       .def("get_joint_velocity", &RobotType::get_joint_velocity)
+      .def("set_joint_acceleration", &RobotType::set_joint_acceleration)
+      .def("get_joint_acceleration", &RobotType::get_joint_acceleration)
       .def("set_velocity_limit", &RobotType::set_velocity_limit)
       .def("set_velocity_limits", &RobotType::set_velocity_limits)
       .def("set_torque_limit", &RobotType::set_torque_limit)

{placo-0.3.6 → placo-0.3.8}/bindings/expose-tools.cpp

@@ -28,7 +28,6 @@ void exposeTools()
   def("wrap_angle", &wrap_angle);
   def("rotation_from_axis", &rotation_from_axis);
   def("frame_yaw", &frame_yaw);
-  def("frame", &frame);
   def("flatten_on_floor", &flatten_on_floor);
 
   exposeStdVector<int>("vector_int");

{placo-0.3.6 → placo-0.3.8}/placo.pyi

@@ -3615,6 +3615,19 @@ class HumanoidRobot:
     :return: the joint current (inner state) value (e.g rad for revolute or meters for prismatic)"""
     ...
 
+  def get_joint_acceleration(
+    self: HumanoidRobot,
+    name: str, # const std::string &
+
+  ) -> float:
+    """Gets the joint acceleration from state.qd. 
+        
+
+    :param name: joint name 
+
+    :return: joint acceleration"""
+    ...
+
   def get_joint_offset(
     self: HumanoidRobot,
     name: str, # const std::string &
@@ -3797,6 +3810,20 @@ class HumanoidRobot:
     :param value: joint value (e.g rad for revolute or meters for prismatic)"""
     ...
 
+  def set_joint_acceleration(
+    self: HumanoidRobot,
+    name: str, # const std::string &
+    value: float, # double
+
+  ) -> None:
+    """Sets the joint acceleration in state.qd. 
+        
+
+    :param name: joint name 
+
+    :param value: joint acceleration"""
+    ...
+
   def set_joint_limits(
     self: HumanoidRobot,
     name: str, # const std::string &
@@ -3926,7 +3953,7 @@ class HumanoidRobot:
     self: HumanoidRobot,
 
   ) -> None:
-    """Update the current kinematics."""
+    """Update internal computation for kinematics (frames, jacobian). This method should be called when the robot state has changed."""
     ...
 
   def update_support_side(
@@ -6007,6 +6034,19 @@ class RobotWrapper:
     :return: the joint current (inner state) value (e.g rad for revolute or meters for prismatic)"""
     ...
 
+  def get_joint_acceleration(
+    self: RobotWrapper,
+    name: str, # const std::string &
+
+  ) -> float:
+    """Gets the joint acceleration from state.qd. 
+        
+
+    :param name: joint name 
+
+    :return: joint acceleration"""
+    ...
+
   def get_joint_offset(
     self: RobotWrapper,
     name: str, # const std::string &
@@ -6176,6 +6216,20 @@ class RobotWrapper:
     :param value: joint value (e.g rad for revolute or meters for prismatic)"""
     ...
 
+  def set_joint_acceleration(
+    self: RobotWrapper,
+    name: str, # const std::string &
+    value: float, # double
+
+  ) -> None:
+    """Sets the joint acceleration in state.qd. 
+        
+
+    :param name: joint name 
+
+    :param value: joint acceleration"""
+    ...
+
   def set_joint_limits(
     self: RobotWrapper,
     name: str, # const std::string &
@@ -6301,7 +6355,7 @@ class RobotWrapper:
     self: RobotWrapper,
 
   ) -> None:
-    """Update the current kinematics."""
+    """Update internal computation for kinematics (frames, jacobian). This method should be called when the robot state has changed."""
     ...
 
   visual_model: any # pinocchio::GeometryModel
@@ -7191,19 +7245,6 @@ def flatten_on_floor(
   ...
 
 
-def frame(
-  matrix: any, # Eigen::Matrix4d
-
-) -> numpy.ndarray:
-  """Makes an Affine3d from a 4x4 matrix (for python bindings) 
-        
-
-  :param matrix: the 4x4 matrix 
-
-  :return: The Affine3d"""
-  ...
-
-
 def frame_yaw(
   rotation: numpy.ndarray, # Eigen::Matrix3d
 

{placo-0.3.6 → placo-0.3.8}/pyproject.toml

@@ -17,12 +17,13 @@ dependencies = [
   "rhoban-cmeel-jsoncpp",
   "meshcat",
   "numpy",
+  "ischedule"
 ]
 description = "PlaCo: Rhoban Planning and Control"
 license = "MIT"
 name = "placo"
 requires-python = ">= 3.8"
-version = "0.3.6"
+version = "0.3.8"
 
 [project.urls]
 changelog = "https://github.com/rhoban/placo/blob/main/CHANGELOG.md"

{placo-0.3.6 → placo-0.3.8}/python/placo_utils/visualization.py

@@ -18,12 +18,15 @@ def get_viewer() -> meshcat.Visualizer:
 
     if viewer is None:
         viewer = meshcat.Visualizer()
+
         print(f"Viewer URL: {viewer.url()}")
 
     return viewer
 
 
-def robot_viz(robot: placo.RobotWrapper, name: str = "robot") -> pin.visualize.MeshcatVisualizer:
+def robot_viz(
+    robot: placo.RobotWrapper, name: str = "robot"
+) -> pin.visualize.MeshcatVisualizer:
     """
     Builds an instance of pinocchio MeshcatVisualizer, which allows to push the model to the meshcat
     visualizer passed as parameter
@@ -31,14 +34,13 @@ def robot_viz(robot: placo.RobotWrapper, name: str = "robot") -> pin.visualize.M
     The robot can further be displayed using:
 
     > viz.display(q)
-
-    :param robot: robot instance
-    :param name: name of the robot
     """
     global robot_names
 
     robot_names[robot] = name
-    viz = pin.visualize.MeshcatVisualizer(robot.model, robot.collision_model, robot.visual_model)
+    viz = pin.visualize.MeshcatVisualizer(
+        robot.model, robot.collision_model, robot.visual_model
+    )
     viz.initViewer(viewer=get_viewer())
     viz.loadViewerModel(name)
 
@@ -51,10 +53,6 @@ cylinders: dict = {}
 def frame_viz(name: str, T: np.ndarray, opacity: float = 1.0) -> None:
     """
     Visualizes a given frame
-
-    :param name: name of the frame
-    :param T: transformation of the frame to the world
-    :param opacity: opacity of the frame
     """
     global cylinders
     vis = get_viewer()
@@ -76,30 +74,60 @@ def frame_viz(name: str, T: np.ndarray, opacity: float = 1.0) -> None:
             )
         obj = cylinders[node_name]
 
-        obj.set_transform(T @ tf.rotation_matrix(*rotate) @ tf.translation_matrix([0, 0.05, 0]))
+        obj.set_transform(
+            T @ tf.rotation_matrix(*rotate) @ tf.translation_matrix([0, 0.05, 0])
+        )
 
 
-def point_viz(name: str, point: np.ndarray, radius: float = 0.01, color: float = 0xFF0000, opacity: float = 1.0) -> None:
+def point_viz(
+    name: str,
+    point: np.ndarray,
+    radius: float = 0.01,
+    color: float = 0xFF0000,
+    opacity: float = 1.0,
+) -> None:
     """
     Prints a point (sphere)
+    """
+    vis = get_viewer()
+    vis["point"][name].set_object(
+        g.Sphere(radius), g.MeshPhongMaterial(color=color, opacity=opacity)
+    )
+    vis["point"][name].set_transform(tf.translation_matrix(point))
+
 
-    :param name: name of the point
-    :param point: point to print
-    :param radius: radius of the point
-    :param color: color of the point
-    :param opacity: opacity of the point
+points_sizes = {}
+
+
+def points_viz(
+    name: str,
+    points: np.ndarray,
+    radius: float = 0.01,
+    color: float = 0xFF0000,
+    opacity: float = 1.0,
+) -> None:
+    """
+    Prints a point (sphere)
     """
+    global points_sizes
     vis = get_viewer()
-    vis["points"][name].set_object(g.Sphere(radius), g.MeshPhongMaterial(color=color, opacity=opacity))
-    vis["points"][name].set_transform(tf.translation_matrix(point))
+    k = 0
+    for point in points:
+        entry = f"{name}_{k}"
+        k += 1
+        vis["points"][name][entry].set_object(
+            g.Sphere(radius), g.MeshPhongMaterial(color=color, opacity=opacity)
+        )
+        vis["points"][name][entry].set_transform(tf.translation_matrix(point))
+    while k < points_sizes.get(name, 0):
+        vis["points"][name][f"{name}_{k}"].delete()
+        k += 1
+    points_sizes[name] = len(points)
 
 
 def robot_frame_viz(robot: placo.RobotWrapper, frame: str) -> None:
     """
-    Draw a frame attached to the robot
-
-    :param robot: robot instance
-    :param frame: frame name
+    Draw a frame from the robot
     """
     node_name = f"{robot_names[robot]}_{frame}"
     frame_viz(node_name, robot.get_T_world_frame(frame))
@@ -109,12 +137,6 @@ steps: int = 0
 
 
 def footsteps_viz(footsteps: placo.Footsteps, T: np.ndarray = np.eye(4)) -> None:
-    """
-    Visualizes humanoid footsteps
-
-    :param footsteps: footsteps to visualize
-    :param T: left transformation to apply to the footsteps
-    """    
     global steps
     vis = get_viewer()
 
@@ -132,23 +154,25 @@ def footsteps_viz(footsteps: placo.Footsteps, T: np.ndarray = np.eye(4)) -> None
             if len(footstep.footsteps) >= 2:
                 color = 0x1111AA
             else:
-                color = 0xFF3333 if str(footstep.footsteps[0].side) == "left" else 0x33FF33
+                color = (
+                    0xFF3333 if str(footstep.footsteps[0].side) == "left" else 0x33FF33
+                )
         else:
             color = 0xFF3333 if str(footstep.side) == "left" else 0x33FF33
 
-        vis["footsteps"][str(k)].set_object(g.LineLoop(g.PointsGeometry(polygon.T), g.MeshBasicMaterial(color=color)))
+        vis["footsteps"][str(k)].set_object(
+            g.LineLoop(g.PointsGeometry(polygon.T), g.MeshBasicMaterial(color=color))
+        )
 
 
 def line_viz(name: str, points: np.ndarray, color: float = 0xFF0000) -> None:
     """
     Prints a line
-
-    :param name: name of the line
-    :param points: points of the line
-    :param color: color of the line
     """
     vis = get_viewer()
-    vis["lines"][name].set_object(g.LineSegments(g.PointsGeometry(points.T), g.LineBasicMaterial(color=color)))
+    vis["lines"][name].set_object(
+        g.LineSegments(g.PointsGeometry(points.T), g.LineBasicMaterial(color=color))
+    )
 
 
 def arrow_viz(
@@ -160,14 +184,8 @@ def arrow_viz(
 ) -> None:
     """
     Prints an arrow
-
-    :param name: name of the arrow
-    :param point_from: origin of the arrow
-    :param point_to: end of the arrow
-    :param color: color of the arrow
-    :param radius: radius of the arrow
     """
-    head_length = radius*3
+    head_length = radius * 3
     vis = get_viewer()
     length = np.linalg.norm(point_to - point_from)
     length = max(1e-3, length - head_length)
@@ -189,43 +207,58 @@ def arrow_viz(
     T_cylinder = T @ tf.translation_matrix(np.array([0, length / 2.0, 0.0]))
     T_head = T @ tf.translation_matrix(np.array([0, length + head_length / 2.0, 0.0]))
 
-    vis["arrows"][name]["cylinder"].set_object(cylinder, g.MeshBasicMaterial(color=color))
+    vis["arrows"][name]["cylinder"].set_object(
+        cylinder, g.MeshBasicMaterial(color=color)
+    )
     vis["arrows"][name]["cylinder"].set_transform(T_cylinder)
     vis["arrows"][name]["head"].set_object(head, g.MeshBasicMaterial(color=color))
     vis["arrows"][name]["head"].set_transform(T_head)
 
+
 previous_contacts: int = 0
 
-def contacts_viz(solver: placo.DynamicsSolver, ratio=0.1, radius=0.005):
-    """
-    Draws the contacts produced by a dynamics solver
 
-    :param solver: solver instance
-    :param ratio: ratio of the wrench to display
-    :param radius: radius of the arrows
-    """    
+def contacts_viz(solver: placo.DynamicsSolver, ratio=0.1, radius=0.005):
     global previous_contacts
     robot = solver.robot
     frames = robot.frame_names()
-    
+
     for k in range(solver.count_contacts()):
         contact = solver.get_contact(k)
 
         if isinstance(contact, placo.PointContact):
             frame_name = frames[contact.position_task().frame_index]
             T_world_frame = robot.get_T_world_frame(frame_name)
-            arrow_viz(f"contact_{k}", T_world_frame[:3, 3], T_world_frame[:3, 3] + contact.wrench * ratio, color=0x00FF00, radius=radius)
+            arrow_viz(
+                f"contact_{k}",
+                T_world_frame[:3, 3],
+                T_world_frame[:3, 3] + contact.wrench * ratio,
+                color=0x00FF00,
+                radius=radius,
+            )
         elif isinstance(contact, placo.Contact6D):
             frame_name = frames[contact.position_task().frame_index]
             T_world_frame = robot.get_T_world_frame(frame_name)
             wrench = T_world_frame[:3, :3] @ contact.wrench[:3]
             origin = T_world_frame[:3, 3] + T_world_frame[:3, :3] @ contact.zmp()
-            arrow_viz(f"contact_{k}", origin, origin + wrench * ratio, color=0x00FFAA, radius=radius)
+            arrow_viz(
+                f"contact_{k}",
+                origin,
+                origin + wrench * ratio,
+                color=0x00FFAA,
+                radius=radius,
+            )
         elif isinstance(contact, placo.ExternalWrenchContact):
             frame_name = frames[contact.frame_index]
             T_world_frame = robot.get_T_world_frame(frame_name)
-            arrow_viz(f"contact_{k}", T_world_frame[:3, 3], T_world_frame[:3, 3] + contact.w_ext[:3] * ratio, color=0xFF2222, radius=radius)
-            
+            arrow_viz(
+                f"contact_{k}",
+                T_world_frame[:3, 3],
+                T_world_frame[:3, 3] + contact.w_ext[:3] * ratio,
+                color=0xFF2222,
+                radius=radius,
+            )
+
     vis = get_viewer()
     while k < previous_contacts:
         k += 1

{placo-0.3.6 → placo-0.3.8}/src/placo/model/robot_wrapper.cpp

@@ -170,6 +170,16 @@ double RobotWrapper::get_joint_velocity(const std::string& name)
   return state.qd[get_joint_v_offset(name)];
 }
 
+void RobotWrapper::set_joint_acceleration(const std::string& name, double value)
+{
+  state.qdd[get_joint_v_offset(name)] = value;
+}
+
+double RobotWrapper::get_joint_acceleration(const std::string& name)
+{
+  return state.qdd[get_joint_v_offset(name)];
+}
+
 void RobotWrapper::set_joint_velocity(const std::string& name, double value)
 {
   state.qd[get_joint_v_offset(name)] = value;

{placo-0.3.6 → placo-0.3.8}/src/placo/model/robot_wrapper.h

@@ -139,6 +139,20 @@ public:
    */
   double get_joint_velocity(const std::string& name);
 
+  /**
+   * @brief Sets the joint acceleration in state.qd
+   * @param name joint name
+   * @param value joint acceleration
+   */
+  void set_joint_acceleration(const std::string& name, double value);
+
+  /**
+   * @brief Gets the joint acceleration from state.qd
+   * @param name joint name
+   * @return joint acceleration
+   */
+  double get_joint_acceleration(const std::string& name);
+
   /**
    * @brief Gets the offset for a given joint in the \ref state (in \ref State.q)
    * @param name joint name
@@ -205,7 +219,8 @@ public:
   State neutral_state();
 
   /**
-   * @brief Update the current kinematics
+   * @brief Update internal computation for kinematics (frames, jacobian). This method should be called when
+   * the robot state has changed.
    */
   void update_kinematics();
 

{placo-0.3.6 → placo-0.3.8}/src/placo/tools/utils.cpp

@@ -73,14 +73,6 @@ Eigen::Matrix3d rotation_from_axis(std::string axis, Eigen::Vector3d vector)
   return pinocchio::exp3(w * theta);
 }
 
-Eigen::Affine3d frame(Eigen::Matrix4d matrix)
-{
-  Eigen::Affine3d result;
-  result.matrix() = matrix;
-
-  return result;
-}
-
 Eigen::Affine3d flatten_on_floor(const Eigen::Affine3d& transformation)
 {
   Eigen::Affine3d flattened = transformation;

{placo-0.3.6 → placo-0.3.8}/src/placo/tools/utils.h

@@ -34,13 +34,6 @@ double frame_yaw(Eigen::Matrix3d rotation);
  */
 Eigen::Matrix3d rotation_from_axis(std::string axis, Eigen::Vector3d vector);
 
-/**
- * @brief Makes an Affine3d from a 4x4 matrix (for python bindings)
- * @param matrix the 4x4 matrix
- * @return The Affine3d
- */
-Eigen::Affine3d frame(Eigen::Matrix4d matrix);
-
 /**
  * @brief Takes a 3D transformation and ensure it is "flat" on the floor
  * (setting z to 0 and keeping only yaw)