Implementation of Fixed Base Task Space Inverse Dynamics #251

isorrentino · 2021-03-26T09:08:02Z

This PR implements the TaskSpaceInverseDynamics interface described in #230 and the QPFixedBaseTSID class which is a concrete implementation of the TSID.

The PR is blocked by #242.

[UPDATE]
The PR is ready to be reviewed @GiulioRomualdi @S-Dafarra @prashanthr05 @traversaro

S-Dafarra · 2021-03-26T09:11:50Z

I think you probably need a rebase

isorrentino · 2021-03-26T11:36:14Z

I'll do a rebase on the master branch after the merging of #242.

S-Dafarra · 2021-03-26T11:39:20Z

I'll do a rebase on the master branch after the merging of #242.

I should have read better the first comment 😁

GiulioRomualdi · 2021-03-31T22:53:39Z

src/TSID/include/BipedalLocomotion/TSID/QPFixedBaseTSID.h

+#define BIPEDAL_LOCOMOTION_QP_FIXED_BASE_TSID_H
+
+#include <memory>
+


Suggested change

#include <optional>

#include <string>

#include <vector>

#include <Eigen/Dense>

#include <iDynTree/KinDynComputations.h>

This is the reason why the compilation fails in windows

#include <optional> was missing

also #include <functional>

GiulioRomualdi · 2021-03-31T22:55:26Z

src/TSID/include/BipedalLocomotion/TSID/QPFixedBaseTSID.h

+
+#include <memory>
+
+#include <BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h>


Suggested change

#include <BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h>

#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>

#include <BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h>

Remember to include what you use 😸

src/TSID/include/BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h

prashanthr05

Initial comments

prashanthr05 · 2021-04-02T09:12:12Z

src/TSID/include/BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h

+/**
+ * State of the TaskSpaceInverseDynamics
+ */


out-of-place documentation?

src/TSID/include/BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h

prashanthr05 · 2021-04-02T09:13:28Z

src/TSID/include/BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h

+};
+struct TSIDState
+{
+    manif::SE3d::Tangent baseAcceleration; /**< Mixed spatial acceleration of the base */


Suggested change

manif::SE3d::Tangent baseAcceleration; /**< Mixed spatial acceleration of the base */

manif::SE3d::Tangent baseAcceleration; /**< Mixed acceleration of the base */

spatial is usually right trivialized.

src/TSID/include/BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h

traversaro · 2021-04-02T09:24:18Z

Removing myself as I guess that @GiulioRomualdi and @prashanthr05 are enough.

prashanthr05 · 2021-04-02T09:26:17Z

src/TSID/include/BipedalLocomotion/TSID/QPFixedBaseTSID.h

+     * Where the generalized robot acceleration is a vector containing the base spatialacceleration
+     (expressed in mixed representation) and the joint accelerations,


Suggested change

* Where the generalized robot acceleration is a vector containing the base spatialacceleration

(expressed in mixed representation) and the joint accelerations,

* Where the generalized robot acceleration is a vector containing the base acceleration

(expressed in mixed representation) and the joint accelerations,

src/TSID/src/QPFixedBaseTSID.cpp

prashanthr05 · 2021-04-02T09:32:28Z

src/TSID/src/QPFixedBaseTSID.cpp

+    std::unordered_map<std::string, TaskWithPriority> tasks;
+    std::vector<std::reference_wrapper<const TaskWithPriority>> constraints;
+    std::vector<std::reference_wrapper<TaskWithPriority>> costs;


Could you add a bit of a documentation here? Also why both constraints and costs have TaskWithPriority type? These are the tasks that are used to build the cost function.

src/TSID/src/QPFixedBaseTSID.cpp

prashanthr05 · 2021-04-02T09:36:43Z

src/TSID/src/QPFixedBaseTSID.cpp

+bool QPFixedBaseTSID::addTask(std::shared_ptr<System::LinearTask> task,
+                                  const std::string& taskName,
+                                  std::size_t priority,
+                                  std::optional<Eigen::Ref<const Eigen::VectorXd>> weight)


check for formatting, cc @GiulioRomualdi can advice on this.

src/TSID/src/QPFixedBaseTSID.cpp

prashanthr05 · 2021-04-02T09:52:01Z

src/TSID/tests/QPFixedBaseTSIDTest.cpp

+    SE3ParameterHandler->setParameter("kp_linear", kp_se3task);
+    SE3ParameterHandler->setParameter("kd_linear", kd_se3task);
+    SE3ParameterHandler->setParameter("kp_angular", kp_se3task);
+    SE3ParameterHandler->setParameter("kd_angular", kd_se3task);


how does the hard-coded 0s in createBaseTask() in FixedBaesQPTSID affect these settings?

S-Dafarra · 2021-04-06T08:51:44Z

Removing myself as I guess that @GiulioRomualdi and @prashanthr05 are enough.

Same for me. Feel free to add me back if you want me to go through it.

GiulioRomualdi · 2021-04-09T15:56:10Z

src/TSID/tests/QPFixedBaseTSIDTest.cpp

+            Eigen::Vector3d gravity;
+            gravity << 0, 0, -9.81;


I don't know if this is the reason why it's not converging but the gravity here is different from the one used in getDesiredReference

I changed it in the version I'm using to understand the problem. The branch is testbranchTSID, the gravity there is set to [0 0 -9.81] also in getDesiredReference.

GiulioRomualdi · 2021-04-16T15:23:46Z

After a F2F discussion with @isorrentino we decided to split the PR in two:

Is for the TSID interface
for the QPFixedBase implementation

Thanks to this split I can start using the interface to implement the TSID for walking, in the meantime @isorrentino can still debug the code and better understand if there are bugs

I will open a PR for 1 by cherry-picking @isorrentino's commit.

…ID class updated

isorrentino · 2021-05-11T08:53:00Z

I rebased the branch on master and I tested it on all the OS. Then, I tested the library with an application that uses the iCub model https://github.com/dic-iit/element_torque-control-via-current/issues/186, both in simulation and with the real robot.
The PR can be reviewed again @GiulioRomualdi @prashanthr05.

GiulioRomualdi

Some minor comments

GiulioRomualdi · 2021-05-11T08:59:04Z

src/TSID/include/BipedalLocomotion/TSID/QPFixedBaseTSID.h

+ * The TSID is here implemented as Quadratic Programming (QP) problem. The user should
+ * set the desired task with the method QPFixedBaseTSID::addTask. Each task has a given
+ * priority. Currently we support only priority equal to 0 or 1. If the task priority is set to 0
+ * the task will be considered as hard task, thus treated as an equality constraint. If the priority


Suggested change

* the task will be considered as hard task, thus treated as an equality constraint. If the priority

* the task will be considered as a hard task, thus treated as a constraint. If the priority

GiulioRomualdi · 2021-05-11T09:02:10Z

src/TSID/include/BipedalLocomotion/TSID/QPFixedBaseTSID.h

+    bool createBaseSE3Task();
+    bool createDynamicsTask();


This two methods should be defined in Impl

GiulioRomualdi · 2021-05-11T09:02:36Z

src/TSID/include/BipedalLocomotion/TSID/QPFixedBaseTSID.h

+     * |:------------------------------------:|:--------:|:--------------------------------------------------------------------------------------------------:|:---------:|
+     * |  `robot_acceleration_variable_name`  | `string` | Name of the variable contained in `VariablesHandler` describing the generalized robot acceleration |    Yes    |
+     * |    `robot_torque_variable_name`      | `string` |         Name of the variable contained in `VariablesHandler` describing the robot torque           |    Yes    |
+     * | `robot_contact_wrench_variable_name` | `string` |      Name of the variable contained in `VariablesHandler` describing the robot contact wrench      |    Yes    |


I would just pass the name of the link that should be considered in contact.
Notice that in your implementation you are assuming that the root link is in contact. If you do not want to add the possibility to have a fixed frame different from you should remove robot_contact_wrench_variable_name

GiulioRomualdi · 2021-05-11T09:07:31Z

src/TSID/src/QPFixedBaseTSID.cpp

+
+    if (!m_pimpl->isKinDynSet)
+    {
+        log()->error("{} The object kinDyn is not set for the base SE3Task.", logPrefix);


Suggested change

log()->error("{} The object kinDyn is not set for the base SE3Task.", logPrefix);

log()->error("{} The object kinDyn is not valid.", logPrefix);

GiulioRomualdi · 2021-05-11T09:08:13Z

src/TSID/src/QPFixedBaseTSID.cpp

+    if (!createBaseSE3Task())
+    {
+        log()->error("{} Error creating se3task for the base.", logPrefix);
+        return false;
+    }
+    if (!createDynamicsTask())
+    {
+        log()->error("{} Error creating task for the joint dynamics.", logPrefix);
+        return false;
+    }


Suggested change

if (!createBaseSE3Task())

{

log()->error("{} Error creating se3task for the base.", logPrefix);

return false;

}

if (!createDynamicsTask())

{

log()->error("{} Error creating task for the joint dynamics.", logPrefix);

return false;

}

if (!m_pimpl->createBaseSE3Task())

{

log()->error("{} Error creating SE3task for the base.", logPrefix);

return false;

}

if (!m_pimpl->createDynamicsTask())

{

log()->error("{} Error creating task for the joint dynamics.", logPrefix);

return false;

}

GiulioRomualdi · 2021-05-11T09:11:45Z

src/TSID/src/QPFixedBaseTSID.cpp

+    m_pimpl->solution.jointTorques
+        = m_pimpl->solver.getSolution().segment(m_pimpl->robotAccelerationVariable.offset
+                                                    + m_pimpl->robotAccelerationVariable.size,
+                                                joints);


Solution contains also the robot acceleration. I think you should fill also that

GiulioRomualdi · 2021-05-11T09:12:55Z

src/TSID/tests/QPFixedBaseTSIDTest.cpp

+            Eigen::Vector3d gravity;
+            gravity << 0, 0, -BipedalLocomotion::Math::StandardAccelerationOfGravitation;;


I think you can move this before for (std::size_t numberOfJoints = 15; numberOfJoints < 40; numberOfJoints += 15)

GiulioRomualdi · 2021-05-11T12:30:54Z

src/TSID/src/QPFixedBaseTSID.cpp

@@ -61,6 +61,64 @@ struct QPFixedBaseTSID::Impl
    std::shared_ptr<SE3Task> baseSE3Task;
    std::shared_ptr<JointDynamicsTask> dynamicsTask;

+    bool createBaseSE3Task() {


Suggested change

bool createBaseSE3Task() {

bool createBaseSE3Task()

{

GiulioRomualdi · 2021-05-11T12:31:08Z

src/TSID/src/QPFixedBaseTSID.cpp

+        return true;
+    }
+
+    bool createDynamicsTask(){


Suggested change

bool createDynamicsTask(){

bool createDynamicsTask()

{

GiulioRomualdi · 2021-05-11T12:49:17Z

Let's wait for CI then I will merge

GiulioRomualdi · 2021-05-11T13:21:54Z

Merging thank you @isorrentino for the effort

prashanthr05 · 2021-05-11T14:08:43Z

Looks like I was a bit too late?

isorrentino requested review from traversaro, prashanthr05, GiulioRomualdi and S-Dafarra March 26, 2021 09:08

isorrentino self-assigned this Mar 26, 2021

isorrentino force-pushed the QPFixedBaseTSID branch from 3cd468e to 6ea23fd Compare March 29, 2021 07:58

GiulioRomualdi reviewed Mar 31, 2021

View reviewed changes

src/TSID/include/BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h Outdated Show resolved Hide resolved

GiulioRomualdi added the enhancement New feature or request label Mar 31, 2021

prashanthr05 reviewed Apr 2, 2021

View reviewed changes

traversaro removed their request for review April 2, 2021 09:23

prashanthr05 reviewed Apr 2, 2021

View reviewed changes

GiulioRomualdi force-pushed the QPFixedBaseTSID branch from 6ea23fd to acaa60c Compare April 2, 2021 11:21

S-Dafarra removed their request for review April 6, 2021 08:51

isorrentino force-pushed the QPFixedBaseTSID branch from 602e5e3 to 3cc9609 Compare April 6, 2021 12:27

GiulioRomualdi reviewed Apr 9, 2021

View reviewed changes

GiulioRomualdi mentioned this pull request Apr 9, 2021

Change the ownership of the KinDynComputation in ContinuousDynamicalSystem #273

Open

GiulioRomualdi mentioned this pull request Apr 16, 2021

Implement TaskSpaceInverseDynamics interface #279

Merged

isorrentino marked this pull request as draft April 19, 2021 10:10

isorrentino added 4 commits May 10, 2021 16:28

Implementation of QPFixedBaseTSID

daeed80

Implementation of QPFixedBaseTSIDTest

fc984ec

Documentation of TaskSpaceInverseDynamics interface and QPFixedBaseTS…

c888062

…ID class updated

Update the CHANGELOG.md

c39b520

GiulioRomualdi and others added 6 commits May 10, 2021 16:28

Update QPFixedBaseTSID class after advanceable refactoring

c90f46d

Update QPFixedBaseTSIDTest after advanceable refactoring

47ed6ac

Changes after review.

e886e35

Priority of SE3Task on the end-effector changed in QPFixedBaseTSIDTest

b4bdf17

Fix bug in QPFixedBaseTSIDTest

bbd448f

Modify QPFixedBaseTSIDTest.cpp after rebasing on master

001755f

isorrentino force-pushed the QPFixedBaseTSID branch from 010bf50 to 001755f Compare May 10, 2021 15:24

isorrentino marked this pull request as ready for review May 11, 2021 08:42

GiulioRomualdi requested review from prashanthr05 and GiulioRomualdi May 11, 2021 08:57

GiulioRomualdi reviewed May 11, 2021

View reviewed changes

Changes after review of QPFixedBaseTSID

0ff6472

isorrentino force-pushed the QPFixedBaseTSID branch from a8bb18a to 0ff6472 Compare May 11, 2021 12:36

GiulioRomualdi merged commit 0c055db into ami-iit:master May 11, 2021

isorrentino deleted the QPFixedBaseTSID branch May 11, 2021 13:49

isorrentino mentioned this pull request May 11, 2021

Implement FixedBase inverse dynamics in TSID #230

Closed

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Implementation of Fixed Base Task Space Inverse Dynamics #251

Implementation of Fixed Base Task Space Inverse Dynamics #251

isorrentino commented Mar 26, 2021 •

edited

S-Dafarra commented Mar 26, 2021

isorrentino commented Mar 26, 2021

S-Dafarra commented Mar 26, 2021

GiulioRomualdi Mar 31, 2021 •

edited

GiulioRomualdi Mar 31, 2021

prashanthr05 Apr 2, 2021

GiulioRomualdi Mar 31, 2021

prashanthr05 left a comment

prashanthr05 Apr 2, 2021

prashanthr05 Apr 2, 2021

traversaro commented Apr 2, 2021

prashanthr05 Apr 2, 2021

prashanthr05 Apr 2, 2021

prashanthr05 Apr 2, 2021

prashanthr05 Apr 2, 2021

S-Dafarra commented Apr 6, 2021

GiulioRomualdi Apr 9, 2021

isorrentino Apr 9, 2021

GiulioRomualdi commented Apr 16, 2021 •

edited

isorrentino commented May 11, 2021

GiulioRomualdi left a comment

GiulioRomualdi May 11, 2021

GiulioRomualdi May 11, 2021

GiulioRomualdi May 11, 2021

GiulioRomualdi May 11, 2021

GiulioRomualdi May 11, 2021

GiulioRomualdi May 11, 2021

GiulioRomualdi May 11, 2021

GiulioRomualdi May 11, 2021

GiulioRomualdi May 11, 2021

GiulioRomualdi commented May 11, 2021

GiulioRomualdi commented May 11, 2021

prashanthr05 commented May 11, 2021

		#define BIPEDAL_LOCOMOTION_QP_FIXED_BASE_TSID_H

		#include <memory>

+#include <optional>
+#include <string>
+#include <vector>
+#include <Eigen/Dense>
+#include <iDynTree/KinDynComputations.h>


		#include <memory>

		#include <BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h>

	#include <BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h>
	#include <BipedalLocomotion/ParametersHandler/IParametersHandler.h>
	#include <BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h>

	manif::SE3d::Tangent baseAcceleration; /*< Mixed spatial acceleration of the base /
	manif::SE3d::Tangent baseAcceleration; /*< Mixed acceleration of the base /

		* Where the generalized robot acceleration is a vector containing the base spatialacceleration
		(expressed in mixed representation) and the joint accelerations,

	* the task will be considered as hard task, thus treated as an equality constraint. If the priority
	* the task will be considered as a hard task, thus treated as a constraint. If the priority

	log()->error("{} The object kinDyn is not set for the base SE3Task.", logPrefix);
	log()->error("{} The object kinDyn is not valid.", logPrefix);

		Eigen::Vector3d gravity;
		gravity << 0, 0, -BipedalLocomotion::Math::StandardAccelerationOfGravitation;;

Implementation of Fixed Base Task Space Inverse Dynamics #251

Implementation of Fixed Base Task Space Inverse Dynamics #251

Conversation

isorrentino commented Mar 26, 2021 • edited

S-Dafarra commented Mar 26, 2021

isorrentino commented Mar 26, 2021

S-Dafarra commented Mar 26, 2021

GiulioRomualdi Mar 31, 2021 • edited

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

prashanthr05 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

traversaro commented Apr 2, 2021

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

S-Dafarra commented Apr 6, 2021

Choose a reason for hiding this comment

Choose a reason for hiding this comment

GiulioRomualdi commented Apr 16, 2021 • edited

isorrentino commented May 11, 2021

GiulioRomualdi left a 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

GiulioRomualdi commented May 11, 2021

GiulioRomualdi commented May 11, 2021

prashanthr05 commented May 11, 2021

isorrentino commented Mar 26, 2021 •

edited

GiulioRomualdi Mar 31, 2021 •

edited

GiulioRomualdi commented Apr 16, 2021 •

edited