Arm moves somewhere when deactivating the cartesian task

[torydebra]

Hi,

I am using xbot2 + cartesianinterface, with the markers.

• I move the arm with the marker
• I deactivate the arm task with rosservice call /cartesian/arm2_8/set_active 0
• Immediately the arm moves somewhere

CentaroCartesioBug_14Nov2021.mp4

What is happening?

Configurations file

I tried with the most basic configuration:

Cartesio conf

solver_options:
    regularization: 0
    back_end: "qpoases"
    front_end: "nhqp"
    nhqp_min_sv_ratio: 0.05

#stack:
    #- ["Postural"]
    #- ["Steering_FL", "Steering_FR", "Steering_HL", "Steering_HR", "Rolling_FL", "Rolling_FR", "Rolling_HL", "Rolling_HR"]
    

stack:
    - ["Car", "Wheel_FL", "Wheel_FR", "Wheel_HR", "Wheel_HL", "Steering_FL", "Steering_FR", "Steering_HL","Steering_HR", "LeftArm", "RightArm"] #, "Gaze"]
    - ["Waist",  "Rolling_FL", "Rolling_FR","Rolling_HL","Rolling_HR","Ankle_FL", "Ankle_FR", "Ankle_HR", "Ankle_HL"]
    - ["Postural"]

constraints: ["JointLimits", "VelocityLimits"]

JointLimits:
    type: "JointLimits"
    
VelocityLimits:
    type: "VelocityLimits"

Gaze:
    type: "Gaze"
    lambda: 0.05
    weight: 1.0

Car:
    type: "Cartesian"
    distal_link: "car_frame"
    lambda: 0.1

Steering_FL:
    type: "CentauroSteering"
    wheel_name: "wheel_1"
    lib_name: "libcentauro_cartesio_addon.so"
    lambda: 0.1
    
Steering_FR:
    type: "CentauroSteering"
    wheel_name: "wheel_2"
    lib_name: "libcentauro_cartesio_addon.so"
    lambda: 0.1
    
Steering_HL:
    type: "CentauroSteering"
    wheel_name: "wheel_3"
    lib_name: "libcentauro_cartesio_addon.so"
    lambda: 0.1
    
Steering_HR:
    type: "CentauroSteering"
    wheel_name: "wheel_4"
    lib_name: "libcentauro_cartesio_addon.so"
    lambda: 0.1
    
Rolling_FL:
    type: "WheelRolling"
    wheel_name: "wheel_1"
    lib_name: "libcentauro_cartesio_addon.so"
    wheel_radius: 0.078
    weight: 1000.0
    
Rolling_FR:
    type: "WheelRolling"
    wheel_name: "wheel_2"
    lib_name: "libcentauro_cartesio_addon.so"
    wheel_radius: 0.078
    weight: 1000.0
    
Rolling_HL:
    type: "WheelRolling"
    wheel_name: "wheel_3"
    lib_name: "libcentauro_cartesio_addon.so"
    wheel_radius: 0.078
    weight: 1000.0
    
Rolling_HR:
    type: "WheelRolling"
    wheel_name: "wheel_4"
    lib_name: "libcentauro_cartesio_addon.so"
    wheel_radius: 0.078
    weight: 1000.0
    
Waist:
    type: "Cartesian"
    distal_link: "pelvis"
    base_link: "car_frame"
    lambda: 0.1
    weight: 10.0
    
Wheel_FL:
    type: "Cartesian"
    distal_link: "wheel_1"
    base_link: "car_frame"
    indices: [0, 1, 2]
    lambda: 0.1
    disabled_joints: ["ankle_yaw_1", "j_wheel_1"]

Wheel_FR:
    type: "Cartesian"
    distal_link: "wheel_2"
    base_link: "car_frame"
    indices: [0, 1, 2]
    lambda: 0.1
    disabled_joints: ["ankle_yaw_2", "j_wheel_2"]

Wheel_HL:
    type: "Cartesian"
    distal_link: "wheel_3"
    base_link: "car_frame"
    indices: [0, 1, 2]
    lambda: 0.1
    disabled_joints: ["ankle_yaw_3", "j_wheel_3"]

Wheel_HR:
    type: "Cartesian"
    distal_link: "wheel_4"
    base_link: "car_frame"
    indices: [0, 1, 2]
    lambda: 0.1
    disabled_joints: ["ankle_yaw_4", "j_wheel_4"]

Ankle_FL:
    type: "Cartesian"
    distal_link: "ankle1_1"
    base_link: "car_frame"
    indices: [3, 4]
    lambda: 0.02
    weight: 10.0

Ankle_FR:
    type: "Cartesian"
    distal_link: "ankle1_2"
    base_link: "car_frame"
    indices: [3, 4]
    lambda: 0.02
    weight: 10.0

Ankle_HL:
    type: "Cartesian"
    distal_link: "ankle1_3"
    base_link: "car_frame"
    indices: [3, 4]
    lambda: 0.02
    weight: 10.0

Ankle_HR:
    type: "Cartesian"
    distal_link: "ankle1_4"
    base_link: "car_frame"
    indices: [3, 4]
    lambda: 0.02
    weight: 10.0
    
LeftArm:
    type: "Cartesian"
    distal_link: "arm1_8"
    base_link: "torso_2"
    lambda: 0.1

RightArm:
    type: "Cartesian"
    distal_link: "arm2_8"
    base_link: "torso_2"
    lambda: 0.1
    
Postural:
    type: "Postural"
    lambda: 0.01
    disabled_joints:
      #- neck_pitch
      #- neck_yaw
      - ankle_yaw_1
      - ankle_yaw_2
      - ankle_yaw_3
      - ankle_yaw_4
      - j_wheel_1
      - j_wheel_2
      - j_wheel_3
      - j_wheel_4
    
Com:
    type: "Com"
    lambda: 0.05
    indices: [0, 1]
    weight: 10

xbot2 conf

XBotInterface:
  urdf_path: $(rospack find centauro_urdf)/urdf/centauro.urdf
  srdf_path: $(rospack find centauro_srdf)/srdf/centauro.srdf

ModelInterface:
  model_type: "RBDL"
  is_model_floating_base: "true"


# hal
xbotcore_device_configs:
    sim: $(rospack find telephysicaloperation)/config/hal/centauro_gz.yaml
    dummy: $(rospack find telephysicaloperation)/config/hal/centauro_dummy.yaml


# threads
xbotcore_threads:
    rt_main:  {sched: fifo , prio: 60, period: 0.001, core_id: 2}
    nrt_main: {sched: other, prio: 0 , period: 0.005}


# plugins
xbotcore_plugins:

    homing:
        thread: rt_main
        type: homing

    ros_io: {thread: nrt_main, type: ros_io}

    ros_control: {thread: nrt_main, type: ros_control, params: {autostart: {type: bool, value: true}}}
    


# global parameters
xbotcore_param:
    /jm/tx_filter_autostart: {value: true, type: bool}
    /jm/tx_filter_cutoff: {value: 2.0, type: double}
    /jm/enable_safety: {value: false, type: bool}
    /xbot_internal/ros_io/call_timeout: {value: 1.0, type: chrono}
    /rt_main/memory_hooks: {value: true, type: bool}

roslaunch

<?xml version="1.0"?>
<launch>

    <arg name="rate" default="100.0"/>
    <arg name="prefix" default=""/>
    <arg name="gui" default="false"/>
    
    <param name="model_description/robot_description" 
        textfile="$(find centauro_cartesio)/configs/urdf/centauro_car.urdf"/>
    
    <param name="model_description/robot_description_semantic"
        textfile="$(find centauro_cartesio)/configs/srdf/centauro_car.srdf"/>
        
    <param name="robot_description" 
        textfile="$(find centauro_urdf)/urdf/centauro.urdf"/>
    
    <param name="robot_description_semantic"
        textfile="$(find centauro_srdf)/srdf/centauro.srdf"/>

    <param name="cartesian/problem_description"
        textfile="$(find telephysicaloperation)/config/cartesio/centauro_car_model_stack.yaml"/>
        
    <rosparam param="cartesian/velocity_whitelist">
        [j_wheel_1, j_wheel_2, j_wheel_3, j_wheel_4]
    </rosparam>


    <include file="$(find cartesian_interface)/launch/cartesio.launch">
        <arg name="rate" value="$(arg rate)"/>
        <arg name="prefix" value="$(arg prefix)"/>
        <arg name="is_model_floating_base" value="true"/>
        <!-- Spawn RviZ interacrive markers -->
        <arg name="markers" value="true"/>
    </include>

    <node type="rviz" name="rviz" pkg="rviz" args="-d $(find telephysicaloperation)/config/rviz/tpoCentauro.rviz" >
    </node> 
    
    <node type="rviz" name="rviz_ci" pkg="rviz" args="-d $(find telephysicaloperation)/config/rviz/tpoCentauro-ci.rviz" >
    </node> 
</launch>

---

@liesrock @alaurenzi

[alaurenzi]

Postural task taking control over the arm?

[torydebra]

Postural task taking control over the arm?

Exactly, indeed this does not happen if I remove the Postural from the stack or I deactivate the Postural online with rosservice call /cartesian/Postural/set_active 0