Questions about PostureTask dimWeight

[mmurooka]

I have two questions about PostureTask.

Often the humanoid's waist joints move largely, producing an unnatural posture. As a result, it seems that changing the stiffness is not enough to avoid this problem, and it is necessary to change the weight of the relevant joints.

First question: The dimWeight() allows changing the weight of each joint, but the order of the joints is not clear. Is the joint order compatible with mb.jointIndexByName? In other words, is the following code correct?

auto postureTask = ctl().getPostureTask(ctl().robot().name());
Eigen::VectorXd dimWeight = postureTask->dimWeight();
dimWeight[ctl().robot().mb().jointIndexByName("WAIST_R")] = 1e2;
dimWeight[ctl().robot().mb().jointIndexByName("WAIST_P")] = 1e2;
dimWeight[ctl().robot().mb().jointIndexByName("WAIST_Y")] = 1e2;
postureTask->dimWeight(dimWeight);

As a proposal, in PostureTask, it would be useful to specify dimWeight by passing a pair of joint name and weight via mc_rtc::Configuration.

Second question: Doesn't the PostureTask managed by the FSM controller have the effect of trying to pin the root link pose at the origin? Although the stiffness and weight are small, this can cause the robot to behave differently at the origin and away from the origin.

[gergondet]

The dimWeight() allows changing the weight of each joint, but the order of the joints is not clear. Is the joint order compatible with mb.jointIndexByName? In other words, is the following code correct?

No, because the task is happening at the variable level and the task has a dimension based on the number of actuated joints whereas jointIndexByName gives you an index in the configuration. You can get the correct index via:

robot().mb().jointPosInDof(robot().mb().jointIndexByName("WAIST_R")) - robot().mb().joint(0).dof();

As a proposal, in PostureTask, it would be useful to specify dimWeight by passing a pair of joint name and weight via mc_rtc::Configuration.

Sounds good. Maybe a jointWeight entry? (note that jointGains/jointStiffness can do this at the stiffness/damping level of the task)

Doesn't the PostureTask managed by the FSM controller have the effect of trying to pin the root link pose at the origin?

The posture task has no effect on the root link (it can only control actuated joints). An end-effector task is added on the root link for that effect but its target is reset to the current state when the FSM enters the idle state.

[mmurooka]

Thanks for the reply. I understand that I have to use jointPosInDof instead of jointIndexByName as you suggested.

However, subtracting the DoF of the root link (i.e., robot().mb().joint(0).dof()) seems to be incorrect. (I tried it, but it did not seem to have any effect on waist joints.) As shown below, the total DoF of the robot and dimWeight dimensions match. Doesn't this suggest that dimWeight includes the DoF of the root link? And again doesn't it suggest that PostureTask affects the root link pose?

auto postureTask = ctl().getPostureTask(ctl().robot().name());
mc_rtc::log::success("postureTask->dimWeight().size(): {}", postureTask->dimWeight().size());
mc_rtc::log::success("ctl().robot().mb().nrDof(): {}", ctl().robot().mb().nrDof());

# robot is JVRC1
[success] postureTask->dimWeight().size(): 50
[success] ctl().robot().mb().nrDof(): 50

[gergondet]

My bad, I should have double checked but yes, the task has the same dimension has the number of dof. In that case you can directly use jointPosInDof without the offset

However, I'm absolutely certain the task has no effect on the root pose:

• the jacobian is explicitly set to zero on the first 6 dof if the robot has a floating base (https://github.com/jrl-umi3218/Tasks/blob/master/src/Tasks.cpp#L809)
• the eval skips the first joint (https://github.com/jrl-umi3218/Tasks/blob/master/src/Tasks.cpp#L831)

[mmurooka]

Thanks for the explanations. I understand now.