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.

Sign up for GitHub

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

Jump to bottom

Add Angular Momentum task & use base-estimator entity to achieve walk in torque control #75

Open

NoelieRamuzat wants to merge 21 commits into stack-of-tasks:devel from NoelieRamuzat:topic/tsid_am_task

Contributor

NoelieRamuzat commented May 25, 2020 •

edited

Loading

This PR is linked to the one in talos-torque-control (see #10).
Add documentations.

NoelieRamuzat added 6 commits

April 6, 2020 16:33


          [Angular Momentum] Add AM task in inverse-dynamic-balance-controller …

27bac45

…entity

Try to use it with talos-torque-control package and trajectories from multicontact_api. Works with walk on spot trajectories.


          [Inv_Dyn] Update references of contacts when they are added


          [Base-estim] Plug base-estimator to inv_dyn entity to have updates on…

c1671d1

… q/v

Remove useless sendMsg functions


          [Doc] Add documentation for walk and bellStep simulation

e233250


          [Inv_Dyn] Clean useless sendMsg

c05584c


          [DDP] Add ddp files to plugin list

15c85a6

NoelieRamuzat mentioned this pull request

Add Angular Momentum task & use base-estimator entity to achieve walk in torque control stack-of-tasks/talos-torque-control#10

Open

NoelieRamuzat marked this pull request as ready for review

May 25, 2020 13:00

andreadelprete reviewed

View reviewed changes

doc/walkRun.md

		@@ -0,0 +1,59 @@
		# Make Pyrene walk in torque control (quasistatic trajectories)

Collaborator

andreadelprete Jun 3, 2020

Wouldn't it be more appropriate to have this documentation in pyrene-torque-control?

Contributor Author

NoelieRamuzat Jul 31, 2020

I also put it in talos-torque-control. But because there were no example of use of sot-torque-control I also put it here.
But I can remove it :)

include/sot/torque_control/imu_offset_compensation.hh

                 /* --- METHODS --- */
                 void update_offset_impl(int iter);
-                void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
-                  logger_.stream(t) << ("[ImuOffsetCompensation-" + name + "] " + msg, t, file, line);
-                }

Collaborator

andreadelprete Jun 3, 2020

Why did you (re)move the sendMsg methods?

Contributor Author

NoelieRamuzat Jul 31, 2020

The sendMsg method have been move into the logger.h class of dynamic-graph to be used uniformly by all packages.
It is imported by:
#include <dynamic-graph/signal-helper.h> which includes:
#include <dynamic-graph/entity.h>

src/inverse-dynamics-balance-controller.cpp Outdated

               void InverseDynamicsBalanceController::removeRightFootContact(const double& transitionTime) {
                 if (m_contactState == DOUBLE_SUPPORT) {
-                  SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO);
+                  SEND_MSG("Remove right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR);

Collaborator

andreadelprete Jun 3, 2020

This doesn't look like an error msg

Contributor Author

NoelieRamuzat Jul 31, 2020

Yes sorry it was for debug purpose I will change this

src/inverse-dynamics-balance-controller.cpp Outdated

                 }
               }
               void InverseDynamicsBalanceController::removeLeftFootContact(const double& transitionTime) {
                 if (m_contactState == DOUBLE_SUPPORT) {
-                  SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO);
+                  SEND_MSG("Remove left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR);

Collaborator

andreadelprete Jun 3, 2020

Same here

src/inverse-dynamics-balance-controller.cpp

                   bool res = m_invDyn->removeRigidContact(m_contactLF->name(), transitionTime);
                   if (!res) {
                     const HQPData& hqpData = m_invDyn->computeProblemData(m_t, m_q_urdf, m_v_urdf);
                     SEND_MSG("Error while remove right foot contact." + tsid::solvers::HQPDataToString(hqpData, false),
                              MSG_TYPE_ERROR);
                   }
                   const double& w_feet = m_w_feetSIN.accessCopy();
-                  m_invDyn->addMotionTask(*m_taskLF, w_feet, 1);
+                  m_invDyn->addMotionTask(*m_taskLF, w_feet, 0);

Collaborator

andreadelprete Jun 3, 2020

Beware that the more tasks you add as constraints, the more you risk getting an unfeasible problem

Contributor Author

NoelieRamuzat Jul 31, 2020 •

edited

Loading

Yes I agree but I achieve better results with this "priority" level in torque control.
However, when I use DCM control to achieve position control (integrate twice the acceleration), I have to put the CoM tasks (-> one for the height and one for the rest) in the cost function.

src/inverse-dynamics-balance-controller.cpp Outdated

               void InverseDynamicsBalanceController::addRightFootContact(const double& transitionTime) {
                 if (m_contactState == LEFT_SUPPORT) {
-                  SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO);
+                  SEND_MSG("Add right foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR);

Collaborator

andreadelprete Jun 3, 2020

Same here

src/inverse-dynamics-balance-controller.cpp Outdated

               void InverseDynamicsBalanceController::addLeftFootContact(const double& transitionTime) {
                 if (m_contactState == RIGHT_SUPPORT) {
-                  SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_INFO);
+                  SEND_MSG("Add left foot contact in " + toString(transitionTime) + " s", MSG_TYPE_ERROR);

Collaborator

andreadelprete Jun 3, 2020

Same here

src/inverse-dynamics-balance-controller.cpp Outdated

+                    assert(v_robot.size() == static_cast<Eigen::VectorXd::Index>(m_robot_util->m_nbJoints + 6));
+                    const dg::Vector& com_measured = m_com_measuredSIN(0);
+                    assert(com_measured.size() == 3);
+                    SEND_MSG("COM_measured: " + toString(com_measured), MSG_TYPE_ERROR);

Collaborator

andreadelprete Jun 3, 2020

Same here

src/simple-inverse-dyn.cpp

		@@ -25,6 +25,8 @@
		#include <dynamic-graph/factory.h>

Collaborator

andreadelprete Jun 3, 2020

Nothing to do with this PR but...what's the difference between the inverse-dynamics-balance-controller and this new simple-inverse-dyn? They look rather similar.

Contributor Author

NoelieRamuzat Jul 31, 2020

Yes they are similar except that there is no possibility to add motion tasks on the hands or feet.
I created this file to have a simpler control scheme.
I use it to understand the huge inverse-dynamics-balance-controller file.
It is used for simple simulations which make the robot moves its CoM (same behaviour as the ex1 and 2 of the jupyter notebooks).

NoelieRamuzat and others added 15 commits

June 16, 2020 10:57


          [Inv_Dyn] Add control mode (vel/torque) and change signal phase (int)

1382c0a

In velocity control use the base estimator to estimate feet positions and CoM position. Use them in the feet and CoM tasks.


          [Inv_Dyn] Wrong init size for m_q_sot (39 -> 38)

30ff3c6


          fix namespace issues

f83a15d

after stack-of-tasks/sot-core#148


          sync submodule

de4ac01


          v1.5.3

a6639c4


          [Angular Momentum] Add AM task in inverse-dynamic-balance-controller …

da0fd7d

…entity

Try to use it with talos-torque-control package and trajectories from multicontact_api. Works with walk on spot trajectories.


          [Inv_Dyn] Update references of contacts when they are added


          [Base-estim] Plug base-estimator to inv_dyn entity to have updates on…

6629e64

… q/v

Remove useless sendMsg functions


          [Doc] Add documentation for walk and bellStep simulation

2e8ba0f


          [Inv_Dyn] Clean useless sendMsg

d5ff701


          [DDP] Add ddp files to plugin list


          [Inv_Dyn] Add control mode (vel/torque) and change signal phase (int)

501baec

In velocity control use the base estimator to estimate feet positions and CoM position. Use them in the feet and CoM tasks.


          [Inv_Dyn] Wrong init size for m_q_sot (39 -> 38)

479e4aa


          Merge remote-tracking branch 'origin/topic/tsid_am_task' into topic/t…

cd75739

…sid_am_task


          [Inv_Dyn] Fix wrong sendMsg type (ERROR to INFO)

d7edbda

Contributor Author

NoelieRamuzat commented Jul 31, 2020

Travis fails on an error of catkin_init_workspace, I don't know why:
./.travis/../travis_custom/custom_build: line 399: catkin_init_workspace: command not found

Member

olivier-stasse commented Jul 31, 2020

Dear @NoelieRamuzat please do not take care of travis output. Right now the output of gitlab.laas.fr is far more informative. During some discussions with @nim65s it is planned to move from travis to github action calling the runner on gitlab.laas.fr
Thanks again for your fixes. I cannot comment them for nom. Maybe @andreadelprete can check your proposal.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment