-
Notifications
You must be signed in to change notification settings - Fork 38
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Implementation of Fixed Base Task Space Inverse Dynamics #251
Conversation
I think you probably need a rebase |
I'll do a rebase on the master branch after the merging of #242. |
I should have read better the first comment 😁 |
3cd468e
to
6ea23fd
Compare
#define BIPEDAL_LOCOMOTION_QP_FIXED_BASE_TSID_H | ||
|
||
#include <memory> | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
#include <optional> | |
#include <string> | |
#include <vector> | |
#include <Eigen/Dense> | |
#include <iDynTree/KinDynComputations.h> |
This is the reason why the compilation fails in windows
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
#include <optional>
was missing
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
also #include <functional>
|
||
#include <memory> | ||
|
||
#include <BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h> |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
#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
Outdated
Show resolved
Hide resolved
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Initial comments
/** | ||
* State of the TaskSpaceInverseDynamics | ||
*/ |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
out-of-place documentation?
src/TSID/include/BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h
Outdated
Show resolved
Hide resolved
}; | ||
struct TSIDState | ||
{ | ||
manif::SE3d::Tangent baseAcceleration; /**< Mixed spatial acceleration of the base */ |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
Outdated
Show resolved
Hide resolved
src/TSID/include/BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h
Outdated
Show resolved
Hide resolved
src/TSID/include/BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h
Outdated
Show resolved
Hide resolved
src/TSID/include/BipedalLocomotion/TSID/TaskSpaceInverseDynamics.h
Outdated
Show resolved
Hide resolved
Removing myself as I guess that @GiulioRomualdi and @prashanthr05 are enough. |
* Where the generalized robot acceleration is a vector containing the base spatialacceleration | ||
(expressed in mixed representation) and the joint accelerations, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
* 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
Outdated
std::unordered_map<std::string, TaskWithPriority> tasks; | ||
std::vector<std::reference_wrapper<const TaskWithPriority>> constraints; | ||
std::vector<std::reference_wrapper<TaskWithPriority>> costs; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
Outdated
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) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
check for formatting, cc @GiulioRomualdi can advice on this.
SE3ParameterHandler->setParameter("kp_linear", kp_se3task); | ||
SE3ParameterHandler->setParameter("kd_linear", kd_se3task); | ||
SE3ParameterHandler->setParameter("kp_angular", kp_se3task); | ||
SE3ParameterHandler->setParameter("kd_angular", kd_se3task); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
how does the hard-coded 0s in createBaseTask()
in FixedBaesQPTSID
affect these settings?
6ea23fd
to
acaa60c
Compare
Same for me. Feel free to add me back if you want me to go through it. |
602e5e3
to
3cc9609
Compare
Eigen::Vector3d gravity; | ||
gravity << 0, 0, -9.81; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
.
After a F2F discussion with @isorrentino we decided to split the PR in two:
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. |
010bf50
to
001755f
Compare
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. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Some minor comments
* 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 |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
* 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 |
bool createBaseSE3Task(); | ||
bool createDynamicsTask(); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This two methods should be defined in Impl
* |:------------------------------------:|:--------:|:--------------------------------------------------------------------------------------------------:|:---------:| | ||
* | `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 | |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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
src/TSID/src/QPFixedBaseTSID.cpp
Outdated
|
||
if (!m_pimpl->isKinDynSet) | ||
{ | ||
log()->error("{} The object kinDyn is not set for the base SE3Task.", logPrefix); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
log()->error("{} The object kinDyn is not set for the base SE3Task.", logPrefix); | |
log()->error("{} The object kinDyn is not valid.", logPrefix); |
src/TSID/src/QPFixedBaseTSID.cpp
Outdated
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; | ||
} |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
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; | |
} |
m_pimpl->solution.jointTorques | ||
= m_pimpl->solver.getSolution().segment(m_pimpl->robotAccelerationVariable.offset | ||
+ m_pimpl->robotAccelerationVariable.size, | ||
joints); |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Solution
contains also the robot acceleration. I think you should fill also that
Eigen::Vector3d gravity; | ||
gravity << 0, 0, -BipedalLocomotion::Math::StandardAccelerationOfGravitation;; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think you can move this before for (std::size_t numberOfJoints = 15; numberOfJoints < 40; numberOfJoints += 15)
src/TSID/src/QPFixedBaseTSID.cpp
Outdated
@@ -61,6 +61,64 @@ struct QPFixedBaseTSID::Impl | |||
std::shared_ptr<SE3Task> baseSE3Task; | |||
std::shared_ptr<JointDynamicsTask> dynamicsTask; | |||
|
|||
bool createBaseSE3Task() { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
bool createBaseSE3Task() { | |
bool createBaseSE3Task() | |
{ |
src/TSID/src/QPFixedBaseTSID.cpp
Outdated
return true; | ||
} | ||
|
||
bool createDynamicsTask(){ |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
bool createDynamicsTask(){ | |
bool createDynamicsTask() | |
{ |
a8bb18a
to
0ff6472
Compare
Let's wait for CI then I will merge |
Merging thank you @isorrentino for the effort |
Looks like I was a bit too late? |
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