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

swuuu · 2022-10-30T20:17:54Z

Description

I added a C++ impedance control method called ImpedanceController::run_local() that is equivalent to compute_impedance_torques() in the python/impedance_controller.py. This made it easier for me to write this file in C++.

How I Tested

I tested this 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:16Z

@Lhumd

…dify c++ impedance algo in world frame, and modify cmake & package.xml so other ROS packages can use this

swuuu · 2023-01-31T04:15:18Z

A few other changes:

In the centroidal controller python file, instead of using the local function to compute the quaternion difference, I called a Pinocchio helper function to do it instead (i.e., pin.log3()). It fixes a bug in this file here, where if SOLO 12 keeps on turning, it will become unstable and fall.
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.

Related to this PR:

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

src/impedance_controller.cpp

CMakeLists.txt

Lhumd · 2023-02-08T03:54:15Z

include/mim_control/impedance_controller.hpp

@@ -4,7 +4,7 @@
 * @copyright Copyright (c) 2020, New York University and Max Planck
 * Gesellschaft
 *
- * @brief This is the implementation for impedance controller between any two
+ * @brief This is the implementation for mim_control_ controller between any two


I think impedance was correct.

Lhumd · 2023-02-08T04:00:19Z

include/mim_control/impedance_controller.hpp

-
-    /** @brief Output torques. */
-    Eigen::VectorXd joint_torques_;
+    class ImpedanceController


Use mpi_cmake_modules to change the format of your code and make it similar to others files in this repo.

It's not easy to review this package because of using a different format.

Are you referring to this: https://github.com/machines-in-motion/mpi_cmake_modules/blob/master/doc/clang_format.md? If so, I couldn't get it to work (command not found even though I sourced my workspace folder). For now, I just un-indented the code.

try with dash instead of underscore
run-clang-format

After running run-clang-format absolute-path-to-file absolute-path-to-folder, I get this error message: run-clang-format: error: Command 'clang-format --version' failed to start: [Errno 2] No such file or directory: 'clang-format'. Do you know what is causing it?

sudo apt install clang-format?

Ah yes, thank you

Lhumd · 2023-02-08T04:02:14Z

include/mim_control/impedance_controller.hpp


-}  // namespace mim_control
+}  // namespace mim_control_


Any reason for changing this line 🤔

No reason, I think I did a search and replace which was wrong. I reverted it back.

Lhumd · 2023-02-08T04:03:38Z

python/mim_control/robot_centroidal_controller.py

@@ -1,10 +1,3 @@
-##################################################################################################################


Any reason for deleting this? You can use a different format if you want it to look like others.

Not sure what happened, I added it back.

Lhumd · 2023-02-08T04:08:07Z

python/mim_control/robot_centroidal_controller.py

@@ -70,7 +63,8 @@ def compute_com_wrench(self, q, dq, des_pos, des_vel, des_ori, des_angvel):
        vcom = np.reshape(np.array(dq[0:3]), (3,))
        Ib = robot.mass(q)[3:6, 3:6]

-        quat_diff = self.quaternion_difference(arr(q[3:7]), arr(des_ori))
+        tmp =  pin.Quaternion(des_ori[3], des_ori[0], des_ori[1], des_ori[2]) * pin.Quaternion(q[6], q[3], q[4], q[5]).conjugate()


Does self.quaternion_difference work? If it doesn't work, please replace your code with the code in the function and here only call the function.

Add impedance control method computed in local frame for c++

896b942

swuuu marked this pull request as draft January 2, 2023 16:36

Fix quaternion difference bug for centroidal controller in python, mo…

40c75ea

…dify c++ impedance algo in world frame, and modify cmake & package.xml so other ROS packages can use this

swuuu marked this pull request as ready for review January 31, 2023 04:09

swuuu mentioned this pull request Jan 31, 2023

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

Open

3 tasks

swuuu changed the title ~~Add impedance control method computed in local frame for c++~~ Add impedance control method computed in local frame for c++, fix centroidal controller quaternion bug, and transform to ROS 2 package Jan 31, 2023

swuuu added 2 commits January 31, 2023 19:55

Update package.xml

f1c66be

Add back license tag to avoid build error

33ef65e

MaximilienNaveau self-requested a review February 7, 2023 11:10

MaximilienNaveau reviewed Feb 7, 2023

View reviewed changes

src/impedance_controller.cpp Outdated Show resolved Hide resolved

MaximilienNaveau reviewed Feb 7, 2023

View reviewed changes

CMakeLists.txt Outdated Show resolved Hide resolved

swuuu added 3 commits February 7, 2023 11:08

Remove underscore

6addafd

Remove ament_cmake

8001af6

Add license

7b178a3

Lhumd self-requested a review February 8, 2023 03:52

Lhumd requested changes Feb 8, 2023

View reviewed changes

Lhumd self-requested a review February 8, 2023 04:12

swuuu added 4 commits February 19, 2023 19:40

Replace mim_control_ with impedance

ce5e3e1

Fix quaternion_difference()

02d70c1

Unindent class

0678ac6

Reformat hpp and cpp files

42912fa

swuuu requested review from MaximilienNaveau and Lhumd and removed request for Lhumd and MaximilienNaveau February 23, 2023 21:33

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

swuuu commented Oct 30, 2022

swuuu commented Oct 30, 2022

swuuu commented Jan 31, 2023 •

edited

Loading

Lhumd Feb 8, 2023

Lhumd Feb 8, 2023

Lhumd Feb 8, 2023

swuuu Feb 20, 2023

MaximilienNaveau Feb 20, 2023

swuuu Feb 22, 2023

MaximilienNaveau Feb 23, 2023

swuuu Feb 23, 2023

Lhumd Feb 8, 2023

swuuu Feb 20, 2023

Lhumd Feb 8, 2023

swuuu Feb 20, 2023

Lhumd Feb 8, 2023

		@@ -1,10 +1,3 @@
		##################################################################################################################

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

Are you sure you want to change the base?

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

Conversation

swuuu commented Oct 30, 2022

Description

How I Tested

I fulfilled the following requirements

swuuu commented Oct 30, 2022

swuuu commented Jan 31, 2023 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

swuuu commented Jan 31, 2023 •

edited

Loading