Spawn object attached to a frame in simulation
ROS Version: Humble
Gazebo Version: Fortress
I'm looking for suggested ways of spawning an object that is attached to a robot arm at run time. That is I'm spawning a URDF model of the arm at startup and then I want to spawn and attach an object to an arm link while running the simulation.
If I use the service interface with the following command:
ign service -s /world/depot/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 1000 --req 'sdf_filename: "/root/calibration_ws/cal_grid/model.sdf", name: "sdf_model"'
And here is my sdf
defined model and joint to attach it to the robot:
<model name="charuco_default">
<link name="charuco_link">
<inertial>
<inertia>
.......
</inertia>
<mass>1.0</mass>
</inertial>
<visual name="visual">
.....
</visual>
<collision name="collision">
.....
</collision>
</link>
</model>
<joint name="cal_joint" type="fixed">
<parent>upper_wrist_link</parent>
<child>charuco_link</child>
<pose>0 0 2 0 0 0</pose>
</joint>
But when I spawn the object it is not attached to my robots link and appears at world 0,0,0
When I look in the UI I can see that the model sdf_model
(which is the name I spawned with) lists the Parent Entry
as 1
which is the same for my robot which I assume means world
is the parent.
If I run: ign model -m sdf_model
I get.
Model: [66]
- Name: sdf_model
- Pose [ XYZ (m) ] [ RPY (rad) ]:
[-0.000000 0.000000 -0.004950]
[0.000001 0.000001 0.000065]
- Link [67]
- Name: charuco_link
- Parent: sdf_model [66]
- Mass (kg): 1.000000
- Inertial Pose [ XYZ (m) ] [ RPY (rad) ]:
[0.000000 0.000000 0.000000]
[0.000000 -0.000000 0.000000]
- Inertial Matrix (kg.m^2):
[1.000000 0.000000 0.000000]
[0.000000 1.000000 0.000000]
[0.000000 0.000000 1.000000]
- Pose [ XYZ (m) ] [ RPY (rad) ]:
[0.000000 0.000000 0.000000]
[0.000000 -0.000000 0.000000]
Any ideas to make it work as desired?
More testing...
So I tried to set my model name of my sdf to the robot's name but get a bunch of errors looking it up.
[ign gazebo-4] [Err] [UserCommands.cc:1194] Error Code 19: Msg: Child frame with name[charuco_default] specified by joint with name[cal_joint] not found in model with name[kinova].
[ign gazebo-4] [Err] [UserCommands.cc:1194] Error Code 24: Msg: FrameAttachedToGraph error, Non-LINK vertex with name [kinova::cal_joint] is disconnected; it should have 1 outgoing edge in MODEL attached_to graph.
[ign gazebo-4] [Err] [UserCommands.cc:1194] Error Code 24: Msg: Graph has __model__ scope but sink vertex named [kinova::cal_joint] does not have FrameType LINK or STATIC_MODEL when starting from vertex with name [kinova::cal_joint].
[ign gazebo-4] [Err] [UserCommands.cc:1194] Error Code 25: Msg: relative_to name[charuco_default] specified by joint with name[cal_joint] does not match a nested model, link, joint, or frame name in model with name[kinova].
[ign gazebo-4] [Err] [UserCommands.cc:1194] Error Code 27: Msg: PoseRelativeToGraph error, Vertex with name [kinova::cal_joint] is disconnected; it should have 1 incoming edge in MODEL relative_to graph.
[ign gazebo-4] [Err] [UserCommands.cc:1194] Error Code 27: Msg: PoseRelativeToGraph unable to find path to source vertex when starting from vertex with id [4].
[ign gazebo-4] [Err] [UserCommands.cc:1194] Error Code 20: Msg: parent frame with name[upper_wrist_link] specified by joint with name[cal_joint] not found in model with name[kinova].
[ign gazebo-4] [Err] [UserCommands.cc:1194] Error Code 19: Msg: child frame with name[charuco_default] specified by joint with name[cal_joint] not found in model with name[kinova].
[ign gazebo-4] [Err] [UserCommands.cc:1194] Error Code 22: Msg: FrameAttachedToGraph unable to find unique frame with name [charuco_default] in graph.
[ign gazebo-4] [Err] [UserCommands.cc:1194] Error Code 22: Msg: FrameAttachedToGraph unable to find unique frame with name [upper_wrist_link] in graph.
[ign gazebo-4] [Err] [UserCommands.cc:1194] Error Code 21: Msg: joint with name[cal_joint] in model with name[kinova] specified parent frame [upper_wrist_link] and child frame [charuco_default] that both resolve to [], but they should resolve to different values.
Asked by MarqRazz on 2022-09-06 21:24:22 UTC
Answers
SDFormat models have to be self contained, i.e, a joint cannot reference a link/frame outside of the model (except world
). So when you tried to spawn your model, it's failing at the SDFormat parsing stage.
This kind of thing is best accomplished using a plugin. My suggestion is to try using the DetachableJoint plugin. You can put the plugin in the object you're spawning at runtime. By default, the DetachableJoint plugin creates a fixed joint between two links when it gets loaded.
Asked by azeey on 2022-09-28 13:40:53 UTC
Comments