how to use virtual_force_publisher.launch for Pepper, which is five (less than six ) joints

[kochigami]

We want to display an arrow which shows a torque 3D direction indicating the largest torque change among pepper's joints, so we try using virtual_force_publisher.launch. However, the result seems unstable. (Red and yellow arrows oscillate.) We guess that is because the length of joint tree is five (ex: I mean the length of joint A -> joint B-> joint C is three), which is less than six.

How does this launch file calculate the result? Can we add an additional joint and make the length of joint tree six or more?
or Is there a good way to show virtual force direction focused on one joint?

We rewrote virtual_force_publisher.launch as follows:

 <launch>
  <arg name="namespace" default="left_endeffector"/>
  <arg name="publish_frequency" default="10.0"/> (changed 50 -> 10)
  <arg name="time_constant" default="0.0"/>
  <arg name="root" default="torso"/> (changed)
  <arg name="tip"  default="l_wrist"/> (changed)
  <arg name="input" default="joint_states"/>
  <group ns="$(arg namespace)">
    <node pkg="virtual_force_publisher" type="virtual_force_publisher" name="virtual_force_publisher"  output="screen">
      <remap from="joint_states" to="/$(arg input)"/>
      <param name="publish_frequency" value="$(arg publish_frequency)"/>
      <param name="time_constant" value="$(arg time_constant)"/>
      <param name="root" value="$(arg root)"/>
      <param name="tip" value="$(arg tip)"/>
    </node>
  </group>
</launch>

After we executed roslaunch virtual_force_publisher virtual_force_publisher.launch, we found the length of joint tree is five like this.

kdl_chain(0) LShoulderPitch
kdl_chain(1) LShoulderRoll
kdl_chain(2) LElbowYaw
kdl_chain(3) LElbowRoll
kdl_chain(4) LWristYaw

Thank you for your help !!

[kochigami]

Sorry for writing issue in a haste. Now, I have looked into virtual_force_publisher.cpp and reviewed robotics class a little.

I understand I can use psuedo jacobian transposation only if link dof is more than six. If link dof is less than six ( three translation and three rotation), endeffector dof is limited to less than six, but I can't find any calculation about this stuation.

Maybe I can change the strategy. What I really need is displaying direction of changing torque. I just focus on shoulder yaw and roll. I gather a lot of data about the direction (though I'm wondering how to do that) and the ratio of shoulder roll torque and shoulder yaw torque. Using Bayesian expectation, I can gain the expected direction. Without using rviz, maybe I can display an arrow in IRT simulator.

[k-okada]

• How many joint does pepper arm have? 5? 6?
  • I'm not sure what happened to virtual force publisher node with 5 dof arm robot. I hope force is ok, but moment is not.
• oscillation of virtual force is normal, specially moment(yellow) please check with baxter (ask @wkentaro or @iory for help)
• easiest way is to set "nice" posture that applied force (by human) is directory applied to some joints. 　(-> https://github.com/jsk-ros-pkg/jsk_robot/issues/348#issuecomment-111910240 )

[kochigami]

I looked again virtual_force_publisher.cpp and calculated the equations through the book about robot control. First, I wondered that the calculation of jac_t_pseudo_inv is wrong and why there is no conditional branch about the comparison of the number of joint dof and end effector dof. However, they were just my misunderstanding.

Now I think this program can be used in all joint and end effector dof, but about pepper, both arrows are unstable even though we kept pulling pepper's arm for a long time.

Thank you very much for your help!!! :)

[kochigami]

後ほど、トルク値の求め方（変換）が適切ではないからではないか、とご指摘いただきました。 確かに、位置のずれをそのままトルクとして使ってしまっていました。申し訳ございません。 もう少しロボティクスを復習して、また試そうと思います。