ROS client can't be initialized: validate() fails for task collision_avoidance

[liesrock]

I was trying to help @aled96 with the debug of an issue reported here: https://github.com/ADVRHumanoids/de_luca_hybrid_planner/issues/20

To make the issue more clear I prepared an example to reproduce the issue of the branch issue_1 here: https://github.com/ADVRHumanoids/cartesio_collision_support/tree/issue_1

Step to reproduce the issue (*centauro_cartesio is needed):

• On the first terminal:

cd PATH_TO_cartesio_collison_support_SRC/launch
roslaunch roslaunch centauro_collision_avoidance_issue.launch

No issue here and the validate function for all the tasks/constraints returns true.

On the second terminal:

cd PATH_TO_cartesio_collision_support_BUILD/devel/lib/cartesio_collision/support
./collision_avoidance_issue 

The error shows up:

Unable to construct TaskRos instance for task 'steering_wheel_1': factory not found for task type 'Task' 
Unable to construct TaskRos instance for task 'steering_wheel_2': factory not found for task type 'Task' 
Unable to construct TaskRos instance for task 'steering_wheel_3': factory not found for task type 'Task' 
Unable to construct TaskRos instance for task 'steering_wheel_4': factory not found for task type 'Task' 
Unable to construct TaskRos instance for task 'rolling_wheel_1': factory not found for task type 'Task' 
Unable to construct TaskRos instance for task 'rolling_wheel_2': factory not found for task type 'Task' 
Unable to construct TaskRos instance for task 'rolling_wheel_3': factory not found for task type 'Task' 
Unable to construct TaskRos instance for task 'rolling_wheel_4': factory not found for task type 'Task' 
Unable to construct TaskRos instance for task 'collision_avoidance': factory not found for task type 'Collision' 
Unable to construct TaskRos instance for task 'collision_avoidance': factory not found for task type 'Task' 
Waiting for all tasks to become valid... 
Waiting for all tasks to become valid... 
Waiting for all tasks to become valid... 
Waiting for all tasks to become valid... 
Waiting for all tasks to become valid... 
Waiting for all tasks to become valid... 
Waiting for all tasks to become valid... 
Waiting for all tasks to become valid... 
Waiting for all tasks to become valid... 
validate() returned false for task 'collision_avoidance' 
terminate called after throwing an instance of 'std::runtime_error'
  what():  Invalid ik problem (see above)
Aborted (core dumped)

The stack in use is the following:

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", "Collision"]

defaults:
 - &local_frame "car_frame"

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

Car:
    type: "Cartesian"
    distal_link: *local_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: *local_frame
    lambda: 0.1
    weight: 10.0

Wheel_FL:
    type: "Cartesian"
    distal_link: "wheel_1"
    base_link: *local_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: *local_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: *local_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: *local_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: *local_frame
    indices: [3, 4]
    lambda: 0.02
    weight: 10.0

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

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

Ankle_HR:
    type: "Cartesian"
    distal_link: "ankle1_4"
    base_link: *local_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

JointLimits:
    type: JointLimits

VelocityLimits:
    type: VelocityLimits

Collision:
    type: CollisionConstraint
    lib_name: libcartesio_collision_support.so
    bound_scaling: 0.1
    distance_threshold: 0.01
    max_pairs: 10
    collision_urdf_path: $(rospack find centauro_urdf)/urdf/capsule/centauro_capsules.urdf
    collision_srdf_path: $(rospack find centauro_srdf)/srdf/capsule/centauro_capsules.srdf
    # pairs:
    #  - [ball1, ball2]
    #  - [wheel_1, wheel_2]
    #  - [hip1_2, arm1_4]
    #  - [knee_1, arm1_4]
    #  - [hip1_2, arm1_5]
    #  - [knee_1, arm1_5]
    #  - [hip2_1, arm1_6]
    #  - [knee_1, arm1_6]
    #  - [hip2_1, ball1]
    #  - [knee_1, ball1]
    #  - [wheel_1, ball1]
    #  - [wheel_1, arm1_6]

The client test exe just does the following:

#include <ros/ros.h>
#include <cartesian_interface/ros/RosClient.h>

int main(int argc, char** argv)
{
    ros::init(argc, argv, "collision_avoidance_issue");

    ros::NodeHandle nh;

    XBot::Cartesian::RosClient _ci_client;

    return 0;

}

I will try to debug more in deep.

fyi @ADVRHumanoids/multi-dof

[liesrock]

Looks like that on the server side the validate call is the expected one on the actual implementation of the CollisionSupport task/bound, instead for the client side the function that gets called is the following:

bool TaskRos::validate()
{
    if(_info.name.empty())
    {
        return false;
    }

    return true;
}

inside here: https://github.com/ADVRHumanoids/CartesianInterface/blob/6d1e2e187e731098ae884e2b704486407930a81d/src/ros/client_api/TaskRos.cpp#L45

Unfortunately _info.name is equal to "" so the function returns false.

Fyi @alaurenzi

[liesrock]

So we are not supposed to use the Collision Avoidance as a task, even though this gets created in the task list, but when we try to

rostopic echo /cartesian/collision_avoidance/task_properties

we get no data: this cause the above issue on the failing validation.

Even if we express the Collision Avoidance as a constraint, the related task gets created, but the task_properties are not streamed in the topic.

[alaurenzi]

I'll look into this asap!