Add method to get the next position for each foot of SOLO, add ROS 2 demos, and transform project to ROS2 package #20

swuuu · 2022-10-30T19:18:09Z

Description

I added a method to get the next position for each foot of SOLO.

How I Tested

I have compiled and tested this repository on SOLO-12 in Gazebo.

I fulfilled the following requirements

All new code is formatted according to our style guide (for C++ run clang-format, for Python, run flake8 and fix all warnings).
All new functions/classes are documented and existing documentation is updated according to changes.
No commented code from testing/debugging is kept (unless there is a good reason to keep it).

swuuu · 2022-10-30T20:18:39Z

@Lhumd

Lhumd · 2022-11-04T22:45:05Z

@swuuu thanks a lot for the improvements.
Could you add reactive_planners with ROS2 that you have written to this pull request or make a new one?

…for ROS 2 usage

…ify cmake to be used for ROS 2

swuuu · 2023-01-31T04:09:38Z

The ROS 2 python and C++ translation of this demo has been added.
I added a package.xml and ament_cmake commands in the CMakeLists.txt for external ROS 2 C++ scripts to link with this package. Essentially, the changes transform this repository into a ROS 2 package so other ROS 2 packages can use it.

This PR is related to this:

Add impedance control method computed in local frame for c++, fix centroidal controller quaternion bug, and transform to ROS 2 package mim_control#29

MaximilienNaveau

Overall this is nice, but I do not understand the need for ament cmake here

CMakeLists.txt

package.xml

Lhumd · 2023-02-07T21:23:15Z

demos/demo_reactive_planners_solo12_step_adjustment_walk.py

+"""Demo file for running the reactive planner on SOLO 12
+The logic is the same as the demo_reactive_planners_solo12_step_adjustment_walk.ipynb
+This file is used for ROS 2. An example of the usage of this class is here: 
+https://github.com/swuuu/ros2_controllers_solo12/blob/master/ros2_control_test_nodes/ros2_control_test_nodes/node.py


Please make sure that you update ros2_controllers_solo12 in the stack-of-tasks organization here, not in your GitHub.
Replace the link above with the one in stack-of-tasks.

The problem is that Olivier Stasse didn't want the packages containing the controllers to be in that repository: stack-of-tasks/ros2_control_solo#1 (review). Perhaps we could ask him to create a new repository?

It's totally fine. It's important to make sure it won't be multiple versions of the same code to avoid bugs:)
Create a new repository on your GitHub ros2_demo_solo for all demo nodes that we will have (or any name that you think is good ). We will add it to McGill organization GitHub in the future.

then I suggest to use a name that represent you organisation like we had with the machines-in-motion group:
mim_control

Lhumd · 2023-02-07T21:27:31Z

demos/demo_reactive_planners_solo12_step_adjustment_walk.py

+class Demo:
+    """
+    Based on https://github.com/machines-in-motion/reactive_planners/blob/master/demos/demo_reactive_planners_solo12_step_adjustment_walk.ipynb
+    The 4 parameters to control the direction of SOLO are y_des, yaw_des, com_des, and v_des.


I suggest changing y_des's name to yaw_velocity_des.

Lhumd · 2023-02-08T00:19:55Z

demos/demo_reactive_planners_solo12_step_adjustment_walk.py

@@ -0,0 +1,317 @@
+"""Demo file for running the reactive planner on SOLO 12


Usually, we write a demo as a runnable file with the possibility of changing parameters based on user preference.
Your Demo class looks easy to understand and useful for users. write a main which call Demo class.
@MaximilienNaveau Do you think it's better to put the Demo class in the python folder?

Not it's fine here.
I would just write a small main at the bottom of the file to run 1 reversion of the demo

Ok, I can write a main() function. But as @Lhumd suggested, could I move this file to the python/reactive_planners/demos folder so that CMake will install() the script so I can use it in another package?

Ah if you want this demo to be part of the public API (other package can use it) then you must move it to the python folder.
But then find another name than demo something saying it's specific to solo and that it's a controller.

In the main function, you want me to run the code in pybullet. Is that correct?

Lhumd · 2023-02-08T01:01:21Z

demos/demo_reactive_planners_solo12_step_adjustment_walk.py

+        :return: torques to send to SOLO
+        """
+        # normalize the quat (maybe not needed)
+        normalized_quat = pin.Quaternion(


If q comes from Pinocchio or simulation means you are sure that q is correct, there is no need to normalize the quaternion.

Lhumd · 2023-02-08T01:02:09Z

demos/demo_reactive_planners_solo12_step_adjustment_walk.py

+        q[6] = normalized_quat[3]
+
+        # transform the base velocity to the local frame
+        curr_orientation = normalized_quat.matrix()


Lhumd · 2023-02-08T01:18:31Z

demos/demo_reactive_planners_solo12_step_adjustment_walk.py

+        xd_com = self.robot.pin_robot.com(q, qdot)[1]
+
+        # update the relevant params for the centroidal controller depending on the command
+        if action == "forward":


Considering this class as a generic class, I prefer not to limit the motion to the actions you wrote here. I suggest writing this part in a more general way without 'action'

just input v_des = [v_x, v_y, v_yaw]

Lhumd · 2023-02-08T02:29:20Z

include/reactive_planners/demo_reactive_planners_solo12_step_adjustment.hpp

+ *
+ * @brief A C++ translation of the demos/demo_reactive_planners_solo12_step_adjustment_walk.ipynb script (the first part)
+ *
+ * The 4 fields to control the direction of SOLO are y_des, yaw_des, com_des, and v_des


Same here for y_des

Lhumd · 2023-02-08T02:57:57Z

src/demo_reactive_planners_solo12_step_adjustment.cpp

+ *
+ * @brief Implement the
+ * reactive_planners::demo_reactive_planners_solo12_step_adjustment class
+ */


As far as I remember, the goal was to write a demo using C++ for solo12 to publish the torque on ros2. I didn't find any demo which does it in your pull request:-?

well it wouldn't really be the place to put that in.
If you wanna ROS2 integration better create a ros2_reactive_planners package or a ros2_control_reactive_planners, whatever name is clear for you to use.

Lhumd · 2023-02-08T03:42:28Z

include/reactive_planners/demo_reactive_planners_solo12_step_adjustment.hpp

+     */
+    compute_torques(Eigen::Matrix<double, 19, 1> &q, Eigen::Matrix<double, 18, 1> &dq, double control_time, const std::string& direction);
+
+    void quadruped_dcm_reactive_stepper_start();


We use Doxygen to generate documentation. Please add comments for every function and variable that you defined.

in vscode there are some tools to help you write the doc for method and attribute

MaximilienNaveau · 2023-02-08T13:05:38Z

demos/demo_reactive_planners_solo12_step_adjustment_walk.py

@@ -0,0 +1,317 @@
+"""Demo file for running the reactive planner on SOLO 12
+The logic is the same as the demo_reactive_planners_solo12_step_adjustment_walk.ipynb
+This file is used for ROS 2. An example of the usage of this class is here: 


Why do you say it is used for ROS2? do you depend on ROS2? do you use rclcpp or rclpy?

Yes, we have a ROS 2 node that uses demo_reactive_planners_solo12_step_adjustment_walk.py and demo_reactive_planners_solo12_step_adjustment_walk.cpp. We use both rclcpp and rclpy. But I agree, both the c++ and Python reactive planner demo files do not need to be used in a ROS2 node.

MaximilienNaveau · 2023-02-08T13:14:30Z

package.xml

+    <license>BSD-3-Clause</license>
+
+    <depend>mpi_cmake_modules</depend>
+    <depend>yaml_utils</depend>


make sure you list all dependencies if you use such package.xml

make sure you add the build_tool you want use and export it:

 <exec_depend condition="$ROS_VERSION == 1">catkin</exec_depend> <exec_depend condition="$ROS_VERSION == 2">ament_cmake</exec_depend> <buildtool_depend>cmake</buildtool_depend> <export> <build_type>cmake</build_type> </export>

MaximilienNaveau · 2023-02-08T13:14:55Z

package.xml

+    <depend>mpi_cmake_modules</depend>
+    <depend>yaml_utils</depend>
+
+</package>


add a new line in the end of all files

MaximilienNaveau · 2023-02-08T13:17:12Z

src/quadruped_dcm_reactive_stepper.cpp

+    Eigen::Vector3d hind_left_next_foot_position_ =
+            biped_stepper_.get_next_support_foot_position() + base_yaw_rot * hl_offset_;
+
+    next_step_feet_pos.row(0) << front_left_next_foot_position_(0), front_left_next_foot_position_(1), front_left_next_foot_position_(2);


next_step_feet_pos.row(0) << front_left_next_foot_position_; works here

MaximilienNaveau · 2023-02-08T13:22:41Z

CMakeLists.txt

  INCLUDES
  DESTINATION include)
+
+# Install the header files so other ROS packages can find them


# Install the header files so other ROS packages can find them should be:
# Install the header files so other packages depending on this one can find them

MaximilienNaveau

Hi, thanks for the work implementing example of the code.
Now if you wanna implement this on a real quadruped I suggest you actually create a ros2_reactive_planners package using ament_cmake this time and use either ros2_control controller API or rclcpp API to publish you input torque to the robot.

Lhumd · 2023-02-08T22:26:44Z

@MaximilienNaveau Thanks for the review.
We will create a new repository as you suggested for ros2 nodes.

…control, and change y_des to yaw_velocity_des

…and add namespace

swuuu added 2 commits October 30, 2022 15:04

Add method to get next step position for each foot of solo

57ae758

Remove .vscode

755b7c4

swuuu added 3 commits January 22, 2023 22:05

Add reactive planner script for C++ and modify cmake and package.xml …

471469a

…for ROS 2 usage

Add reactive planner python and C++ scripts and add package.xml & mod…

4688e82

…ify cmake to be used for ROS 2

Merge branch 'master' of github.com:swuuu/reactive_planners

7c5054b

swuuu changed the title ~~Add method to get the next position for each foot of SOLO~~ Add method to get the next position for each foot of SOLO and transform project to ROS2 package Jan 31, 2023

swuuu changed the title ~~Add method to get the next position for each foot of SOLO and transform project to ROS2 package~~ Add method to get the next position for each foot of SOLO, add ROS 2 demos, and transform project to ROS2 package Jan 31, 2023

swuuu mentioned this pull request Jan 31, 2023

Add impedance control method computed in local frame for c++, fix centroidal controller quaternion bug, and transform to ROS 2 package machines-in-motion/mim_control#29

Open

3 tasks

swuuu added 3 commits January 31, 2023 19:49

Update package.xml

e1e32a6

Add copyright info

94769e2

Add back license tag to avoid build error

97c4aea

MaximilienNaveau requested changes Feb 7, 2023

View reviewed changes

CMakeLists.txt Outdated Show resolved Hide resolved

package.xml Outdated Show resolved Hide resolved

package.xml Outdated Show resolved Hide resolved

Lhumd self-requested a review February 7, 2023 14:26

swuuu added 2 commits February 7, 2023 17:30

Remove ament_cmake

770748d

Add license

89aa097

Lhumd requested changes Feb 8, 2023

View reviewed changes

MaximilienNaveau reviewed Feb 8, 2023

View reviewed changes

MaximilienNaveau requested changes Feb 8, 2023

View reviewed changes

swuuu added 4 commits February 9, 2023 14:21

Add demo_reactive_planners_solo12 file as source file, link with mim_…

ab13ef1

…control, and change y_des to yaw_velocity_des

Remove normalizing quaternion step

9d6b1ec

Rename y_des to yaw_velocity_des in cpp files

e960493

Rename demo files, add reactive_planners_controller_solo12 to CMake, …

409cf57

…and add namespace

swuuu added 5 commits February 12, 2023 22:49

Remove ROS comment in CMake file

bf0529d

Simplify vector assignment

a4c9a91

Add newline to end of file

bc43fc7

Add missing dependencies in package.xml

fd742f3

Rename to reactive_planners_controller_solo and refactor v_des

ebb1502

		@@ -0,0 +1,317 @@
		"""Demo file for running the reactive planner on SOLO 12

Uh oh!

Add method to get the next position for each foot of SOLO, add ROS 2 demos, and transform project to ROS2 package #20

Are you sure you want to change the base?

Add method to get the next position for each foot of SOLO, add ROS 2 demos, and transform project to ROS2 package #20

Uh oh!

Conversation

swuuu commented Oct 30, 2022

Description

How I Tested

I fulfilled the following requirements

Uh oh!

swuuu commented Oct 30, 2022

Uh oh!

Lhumd commented Nov 4, 2022 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

swuuu commented Jan 31, 2023 • edited Loading Uh oh! There was an error while loading. Please reload this page.

Uh oh!

Uh oh!

MaximilienNaveau left a comment

Choose a reason for hiding this comment

Uh oh!

Uh oh!

Uh oh!

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Uh oh!

Choose a reason for hiding this comment

Lhumd commented Nov 4, 2022 •

edited

Loading

swuuu commented Jan 31, 2023 •

edited

Loading