pal-robotics/gazebo_ros_link_attacher

Attach two Gazebo models with a virtual joint in a generalized grasp hack

Attach two Gazebo models with a virtual joint in a generalized grasp hack

Build

mkdir -p gazebo_link_attacher_ws/src
cd gazebo_link_attacher_ws/src
catkin_init_workspace
git clone https://github.com/pal-robotics/gazebo_ros_link_attacher.git
cd ..
catkin_make
source devel/setup.bash

Launch

roslaunch gazebo_ros_link_attacher test_attacher.launch

Empty world with the plugin libgazebo_ros_link_attacher.so loaded (in the worlds folder).

Which provides the /link_attacher_node/attach service to specify two models and their links to be attached.

And /link_attacher_node/detach service to specify two models and their links to be detached.

Run demo

In another shell, be sure to do source devel/setup.bash of your workspace.

rosrun gazebo_ros_link_attacher spawn.py

Three cubes will be spawned.

rosrun gazebo_ros_link_attacher attach.py

The cubes will be attached all between themselves as (1,2), (2,3), (3,1). You can move them with the GUI and you'll see they will move together.

rosrun gazebo_ros_link_attacher detach.py

The cubes will be detached and you can move them separately again.

You can also spawn items with the GUI and run a rosservice call:

rosservice call /link_attacher_node/attach "model_name_1: 'unit_box_1'
link_name_1: 'link'
model_name_2: 'unit_sphere_1'
link_name_2: 'link'"

And same thing to detach:

rosservice call /link_attacher_node/detach "model_name_1: 'unit_box_1'
link_name_1: 'link'
model_name_2: 'unit_sphere_1'
link_name_2: 'link'"

Current status

It works!

Currently it crashes with:

***** Internal Program Error - assertion (self->inertial != __null) failed in static void gazebo::physics::ODELink::MoveCallback(dBodyID):
/tmp/buildd/gazebo4-4.1.3/gazebo/physics/ode/ODELink.cc(178): Inertial pointer is NULL
Aborted (core dumped)

Which I've only seen this other useful information: Bitbucket gazebo removing moving model with ode friction fails. But it didn't help me solve my crash. I guess when attaching one model to the other it removes the second one to re-create it attached to the first or something like that.

And also this other issue: Visualizing dynamically created joints made me add a couple of lines more.

The method to attach the links is based on the grasp hack of the Gripper in gazebo/physics: Gripper.hh Gripper.cc

Which is to create a revolute joint on the first model (with range of motion 0) linking a link on the first model and a link on the second model.

gazebo_ros_link_attacher's Issues

Build errors for gazebo 6

Hey, I assume you have build this only against gazebo 2? I get a lot of build errors for gazebo 6.6

No License specified

It would be great if you could add a license to this package. It's a pretty great tool, thanks for developing this

Missing Attach.msg file

Where it is supossed to be the Attach.msg file that is imported from the python scripts?

Getting error messages on ros kinetic

Does this package support ros kinetic? cos while catkin_make, I get the following error messages:

_In member function ‘virtual void gazebo::GazeboRosLinkAttacher::Load(gazebo::physics::WorldPtr, sdf::ElementPtr)’:
/home/ssgeek41/ros_ws/src/gazebo_ros_link_attacher-melodic-devel/src/gazebo_ros_link_attacher.cpp:37:34: error: ‘class gazebo::physics::World’ has no member named ‘Physics’
this->physics = this->world->Physics();
^
/home/ssgeek41/ros_ws/src/gazebo_ros_link_attacher-melodic-devel/src/gazebo_ros_link_attacher.cpp: In member function ‘bool gazebo::GazeboRosLinkAttacher::attach(std::__cxx11::string, std::__cxx11::string, std::__cxx11::string, std::_cxx11::string)’:
/home/ssgeek41/ros_ws/src/gazebo_ros_link_attacher-melodic-devel/src/gazebo_ros_link_attacher.cpp:67:40: error: ‘class gazebo::physics::World’ has no member named ‘ModelByName’
physics::BasePtr b1 = this->world->ModelByName(model1);
^
/home/ssgeek41/ros_ws/src/gazebo_ros_link_attacher-melodic-devel/src/gazebo_ros_link_attacher.cpp:74:40: error: ‘class gazebo::physics::World’ has no member named ‘ModelByName’
physics::BasePtr b2 = this->world->ModelByName(model2);
^
In file included from /opt/ros/kinetic/include/ros/ros.h:40:0,
from /home/ssgeek41/ros_ws/src/gazebo_ros_link_attacher-melodic-devel/src/gazebo_ros_link_attacher.cpp:2:
/home/ssgeek41/ros_ws/src/gazebo_ros_link_attacher-melodic-devel/src/gazebo_ros_link_attacher.cpp:96:100: error: ‘class gazebo::physics::Inertial’ has no member named ‘Mass’
1 inertia is not NULL, for example, mass is: " << l1->GetInertial()->Mass())

About missing license information.

Hi,

Do we have any license information for this ROS package? If not then can you please add the license?

Thanks

Objects slips from gripper during Gazebo simulation / How to use gazebo_ros_link_attacher ?

I'm trying to simulate a simple scenario of pick and place for my UR5 ARM robot but i'm encoutering some problems during grasp. In particular, when grasping the gripper starts shaking untill the object slips out of the gripper the gripper seems to explode as can be seen in Video I am using effort_controllers. Please help me to solve the issue or please let me known how can I use gazebo_ros_link_attacher in my project.

</gazebo>
    <!--using the grasp plugin so that the gripper can grasp things in simulation-->
    <gazebo>
        <plugin name="gazebo_grasp_fix" filename="libgazebo_grasp_fix.so">
            <arm>
                <arm_name>ur5</arm_name>
                <palm_link>wrist_3_link</palm_link>
                <gripper_link>gripper_finger1_finger_tip_link</gripper_link>
                <gripper_link>gripper_finger2_finger_tip_link</gripper_link>
                <gripper_link>gripper_finger1_inner_knuckle_link</gripper_link>
                <gripper_link>gripper_finger2_inner_knuckle_link</gripper_link>
                <gripper_link>gripper_finger1_knuckle_link</gripper_link>
                <gripper_link>gripper_finger2_knuckle_link</gripper_link>
            </arm>
            <forces_angle_tolerance>100</forces_angle_tolerance>
            <update_rate>10</update_rate>
            <grip_count_threshold>4</grip_count_threshold>
            <max_grip_count>8</max_grip_count>
            <release_tolerance>0.005</release_tolerance>
            <disable_collisions_on_attach>false</disable_collisions_on_attach>
            <contact_topic>__default_topic__</contact_topic>
        </plugin>
    </gazebo>

Joint with world

Hey
I have a mobile robot. When I stop it on a location in world it slips from its original location after some time.
So basically, I want to use your code to create a joint between base_link of the robot and the world (with some fix frame). This will make the robot fix. When I want to move it again, I will remove the joint. I can do this in robot xacro file but then I cannot move it at all. So, I want to do it dynamically. So please let me know can I acheive it with this plugin.
Thank you for your time.

Can't compile with ros melodic

Errors << gazebo_ros_link_attacher:make /home/mehdi/catkin_ws/logs/gazebo_ros_link_attacher/build.make.000.log
/home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp: In member function ‘virtual void gazebo::GazeboRosLinkAttacher::Load(gazebo::physics::WorldPtr, sdf::ElementPtr)’:
/home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp:36:34: error: ‘class gazebo::physics::World’ has no member named ‘GetPhysicsEngine’; did you mean ‘SetPhysicsEnabled’?
this->physics = this->world->GetPhysicsEngine();
^~~~~~~~~~~~~~~~
SetPhysicsEnabled
/home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp: In member function ‘bool gazebo::GazeboRosLinkAttacher::attach(std::__cxx11::string, std::__cxx11::string, std::__cxx11::string, std::__cxx11::string)’:
/home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp:66:40: error: ‘class gazebo::physics::World’ has no member named ‘GetByName’; did you mean ‘BaseByName’?
physics::BasePtr b1 = this->world->GetByName(model1);
^~~~~~~~~
BaseByName
/home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp:72:40: error: ‘class gazebo::physics::World’ has no member named ‘GetByName’; did you mean ‘BaseByName’?
physics::BasePtr b2 = this->world->GetByName(model2);
^~~~~~~~~
BaseByName
In file included from /opt/ros/melodic/include/ros/ros.h:40:0,
from /home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp:2:
/home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp:94:100: error: ‘class gazebo::physics::Inertial’ has no member named ‘GetMass’; did you mean ‘SetMass’?
ROS_DEBUG_STREAM("link1 inertia is not NULL, for example, mass is: " << l1->GetInertial()->GetMass());
^
/opt/ros/melodic/include/ros/console.h:356:64: note: in definition of macro ‘ROSCONSOLE_PRINT_STREAM_AT_LOCATION_WITH_FILTER’
rosconsole_print_stream_at_location_with_filter__ss << args;
^~~~
/opt/ros/melodic/include/ros/console.h:404:7: note: in expansion of macro ‘ROSCONSOLE_PRINT_STREAM_AT_LOCATION’
ROSCONSOLE_PRINT_STREAM_AT_LOCATION(args);
^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/opt/ros/melodic/include/ros/console.h:577:43: note: in expansion of macro ‘ROS_LOG_STREAM_COND’
#define ROS_LOG_STREAM(level, name, args) ROS_LOG_STREAM_COND(true, level, name, args)
^~~~~~~~~~~~~~~~~~~
/opt/ros/melodic/include/rosconsole/macros_generated.h:59:32: note: in expansion of macro ‘ROS_LOG_STREAM’
#define ROS_DEBUG_STREAM(args) ROS_LOG_STREAM(::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, args)
^~~~~~~~~~~~~~
/home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp:94:9: note: in expansion of macro ‘ROS_DEBUG_STREAM’
ROS_DEBUG_STREAM("link1 inertia is not NULL, for example, mass is: " << l1->GetInertial()->GetMass());
^
/home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp:106:100: error: ‘class gazebo::physics::Inertial’ has no member named ‘GetMass’; did you mean ‘SetMass’?
ROS_DEBUG_STREAM("link2 inertia is not NULL, for example, mass is: " << l2->GetInertial()->GetMass());
^
/opt/ros/melodic/include/ros/console.h:356:64: note: in definition of macro ‘ROSCONSOLE_PRINT_STREAM_AT_LOCATION_WITH_FILTER’
rosconsole_print_stream_at_location_with_filter__ss << args;
^~~~
/opt/ros/melodic/include/ros/console.h:404:7: note: in expansion of macro ‘ROSCONSOLE_PRINT_STREAM_AT_LOCATION’
ROSCONSOLE_PRINT_STREAM_AT_LOCATION(args);
^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/opt/ros/melodic/include/ros/console.h:577:43: note: in expansion of macro ‘ROS_LOG_STREAM_COND’
#define ROS_LOG_STREAM(level, name, args) ROS_LOG_STREAM_COND(true, level, name, args)
^~~~~~~~~~~~~~~~~~~
/opt/ros/melodic/include/rosconsole/macros_generated.h:59:32: note: in expansion of macro ‘ROS_LOG_STREAM’
#define ROS_DEBUG_STREAM(args) ROS_LOG_STREAM(::ros::console::levels::Debug, ROSCONSOLE_DEFAULT_NAME, args)
^~~~~~~~~~~~~~
/home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp:106:9: note: in expansion of macro ‘ROS_DEBUG_STREAM’
ROS_DEBUG_STREAM("link2 inertia is not NULL, for example, mass is: " << l2->GetInertial()->GetMass());
^
/home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp:118:27: error: ‘math’ has not been declared
j.joint->Load(l1, l2, math::Pose());
^~~~
/home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp:136:14: error: ‘class gazebo::physics::Joint’ has no member named ‘SetHighStop’
j.joint->SetHighStop(0, 0);
^~~~~~~~~~~
/home/mehdi/catkin_ws/src/gazebo_ros_link_attacher/src/gazebo_ros_link_attacher.cpp:138:14: error: ‘class gazebo::physics::Joint’ has no member named ‘SetLowStop’; did you mean ‘SetPosition’?
j.joint->SetLowStop(0, 0);
^~~~~~~~~~
SetPosition
make[2]: *** [CMakeFiles/gazebo_ros_link_attacher.dir/src/gazebo_ros_link_attacher.cpp.o] Error 1
make[1]: *** [CMakeFiles/gazebo_ros_link_attacher.dir/all] Error 2
make: *** [all] Error 2

Is your code only compatible with older Gazebo versions?

keep a certain distance between two links after attach active

Hello friend,
-- tried this pkg, there is one situation, if before I attach these three cubes, I manually move one to a position, then I attach them, the problem is it will just keep the relative pose after I moved one cube, is there a way I can set the link's relative pose when attach them ? Please let me know ur advics

thanks in advance
Leo

dynamic casting no-sense

I'm using your code as base of a robot specific plugin. I've realized that you are doing a share_ptr dynamic conversion between BasePtr and ModelPtr. I think you have to avoid that. It generates dangerous memory issues.

I think you sould change:
"""
physics::BasePtr b1 = this->world->ModelByName(model1);

if (b1 == NULL){
  ROS_ERROR_STREAM(model1 << " model was not found");
  return false;
}

ROS_DEBUG_STREAM("Casting into ModelPtr");
physics::ModelPtr m1(dynamic_cast<physics::Model*>(b1.get()));
j.m1 = m1;

"""
Directly to
"""
physics::ModelPtr m1 = this->world->ModelByName(model1);
if(!m1){ROS_ERROR_STREAM(model1 << " model was not found");
return false;
"""

If you do not have a list of attached joints, the pluging would delete the module, because of the BasePtr shared_pointer.

ROS2 version?

Hello,
is there a version of this tool for ROS2? I can try to adapt it to ROS2. Besides rewriting CMakelists.txt and using rclcpp functionalities, do you think major code refactoring is needed for the Gazebo functions?

Thank you

Not able to detach

I wanted to create a temporary joint with respect to ground. So using gazebo_ros_link_attacher I was able to attach model object with ground(world model) but not able to detach it.

Hierarchy attachment

I have TIAGo and some objects (boxes) in the scene, something like below from Gazebo:

Models
     > tiago
          - arm1
          - arm2
          - link1
          - link2
      > box1
      > box2

I am able to attach box1 and box2 using the below
rosservice call /link_attacher_node/attach "model_name_1: 'box1'
link_name_1: 'link'
model_name_2: 'box2'
link_name_2: 'link'"

but I would like to attach box1 with link1 of TIAGo

I tried

1-
rosservice call /link_attacher_node/attach "model_name_1: 'box1'
link_name_1: 'link'
model_name_2: 'tiago/link1'
link_name_2: 'link'"

2-
rosservice call /link_attacher_node/attach "model_name_1: 'box1'
link_name_1: 'link'
model_name_2: 'link1'
link_name_2: 'link'"

3-
rosservice call /link_attacher_node/attach "model_name_1: 'box1'
link_name_1: 'link'
model_name_2: 'Models/tiago/link1'
link_name_2: 'link'"

4-
rosservice call /link_attacher_node/attach "model_name_1: 'box1'
link_name_1: 'link'
model_name_2: 'tiago::link1'
link_name_2: 'link'"

I am not able to attach it, basically, Gazebo prints "model not found" and in turn could not make the attach

The link cannot be moved after it is attached and detached to a static model

I use https://github.com/Pitrified/gazebo_r... to attach my robot‘s tail link to ground_plane's link,but the head link cannot be moved after it is detached from the ground_plane's link. Not native English Speaker，Thanks！

The head link appear to be fixed at the joint points.

Attach, detach and reattach

I am currently having problems with attaching multiple times. When I spawn a model, attach its link to another model's link it works fine. Detaching also works fine. But when I try to reattach those two links again it doesn't work and I don't see any error message. I am using the melodic-devel branch so I was wondering if this also happens in the master branch.