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

liesrock · 2021-08-27T09:32:54Z

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

The text was updated successfully, but these errors were encountered:

liesrock · 2021-08-27T09:55:56Z

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 · 2021-08-27T10:04:28Z

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 · 2021-09-02T14:16:39Z

I'll look into this asap!

liesrock self-assigned this Aug 27, 2021

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

liesrock commented Aug 27, 2021 •

edited

Loading

liesrock commented Aug 27, 2021

liesrock commented Aug 27, 2021 •

edited

Loading

alaurenzi commented Sep 2, 2021

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

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

Comments

liesrock commented Aug 27, 2021 • edited Loading

liesrock commented Aug 27, 2021

liesrock commented Aug 27, 2021 • edited Loading

alaurenzi commented Sep 2, 2021

liesrock commented Aug 27, 2021 •

edited

Loading

liesrock commented Aug 27, 2021 •

edited

Loading