Arm moves somewhere when deactivating the cartesian task

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

https://user-images.githubusercontent.com/26459008/141683328-890ce6f1-7190-4a28-b420-c5443b604a09.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

ADVRHumanoids / CartesianInterface

Arm moves somewhere when deactivating the cartesian task #65

Configurations file