Gazebo | Ignition | Community
Ask Your Question
1

[ROS2] Setting initial joint position at spawn

asked 2021-11-18 16:23:55 -0500

Luczia gravatar image

Hi!

I'am looking for a way to spawn a 2Dof robotics Arm (SCARA type) in gazebo from ROS2 at a desired Joint Position. The current URDF has the arm straight and I would like to spawn the arm "half closed".

image description

I'am not sure how to do it in ROS2.

If I understand correctly, there might be two solutions :

  1. Adding an argument when calling the spawn node. It used to be -J with ROS 1. But I'am not even sure it is implemented yet according to the spawn_entity.py repo and the git issue #779

  2. Using a Gazebo Service (something like /set_model_configuration) but I can't find such solution/service in the [ocumentation:

As a reference, here is my spawn code (ROS2 Foxy, Gazebo 11) :

import os
import sys
import rclpy
from gazebo_msgs.srv import SpawnEntity

def main(args=None):
    # Get input arguments from user
    argv = sys.argv[1:] 
    rclpy.init(args=args)
    node = rclpy.create_node('entity_spawner')
    cli = node.create_client(SpawnEntity, '/spawn_entity')


    req = SpawnEntity.Request()
    req.name = argv[0]
    req.xml = argv[1]
    req.robot_namespace = argv[2]
    req.reference_frame = argv[3]

    req.initial_pose.position.x = float(argv[4])
    req.initial_pose.position.y = float(argv[5])
    req.initial_pose.position.z = float(argv[6])
    req.initial_pose.orientation.w = float(argv[7])
    req.initial_pose.orientation.x = float(argv[8])
    req.initial_pose.orientation.y = float(argv[9])
    req.initial_pose.orientation.z = float(argv[10])

    while not cli.wait_for_service(timeout_sec=1.0):
        node.get_logger().info('service not available, waiting again...')

    future = cli.call_async(req)
    rclpy.spin_until_future_complete(node, future)

    if future.result() is not None:
        node.get_logger().info(
            'Result ' + str(future.result().success) + " " + future.result().status_message)
    else:
        node.get_logger().info('Service call failed %r' % (future.exception(),))

    node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

Do you happen to have any code example or solution to set an angle at spawn for my joint1 and joint2 ? (btw I'd also be interested to know how to to do it in Ignition for future reference).

URDF reference :

<joint name="joint1" type="continuous">
  <origin rpy="0 0 0" xyz="0 0 0.195"/>
  <parent link="base_link"/>
  <child link="arm_link"/>
  <axis xyz="0.0 -0.0 1.0"/>
  <dynamics damping="10.0" friction="2.0" />
</joint>

<joint name="joint2" type="continuous">
  <origin rpy="0 0 0" xyz="0.263 0.0 0.0"/>
  <parent link="arm_link"/>
  <child link="forearm_link"/>
  <axis xyz="0.0 0.0 1.0"/>
  <dynamics damping="10.0" friction="2.0" />
</joint>
edit retag flag offensive close merge delete

3 Answers

Sort by ยป oldest newest most voted
0

answered 2022-01-26 04:43:38 -0500

christophfroehlich gravatar image

Hi there, have you found any solution yet?

According to the your links above there's still no update on that.

How we implemented it now is that we set an joint offset within the origin tag in the urdf file, but its not the most elegant way.

edit flag offensive delete link more

Comments

There is also a separate older unanswered feature request https://github.com/ros-simulation/gaz...

christophfroehlich gravatar imagechristophfroehlich ( 2022-01-26 05:55:16 -0500 )edit

it seems that there is a solution with gazebo_ros2_control: https://github.com/ros-simulation/gaz... but it is available for ros2 rolling only.

christophfroehlich gravatar imagechristophfroehlich ( 2022-02-07 11:29:28 -0500 )edit

Well, it seems that the fixes your are referring to are for gazebo classics but not Gazebo Sim (ex ignition). I'm probably missing something but I haven't figures out how to initialize the joints angles at this point. Does anyone have an example working on ROS2/Gazebo Sim ?

Luczia gravatar imageLuczia ( 2023-05-29 19:36:41 -0500 )edit
0

answered 2022-12-09 14:07:12 -0500

Luczia gravatar image

At this point I still haven't found a solution.

One can add a 0.05 tag to the <state_interface> but it only act as an initial joint_trajectory_command ; i.e : the joint reaches target position as soon as the controllers are initialized.

 <joint name="joint1">
  <command_interface name="position">
    <param name="min">-3.14</param>
    <param name="max">3.14</param>
  </command_interface>
  <state_interface name="position">
    <param name="initial_value">0.05</param>
  </state_interface>
  <state_interface name="velocity"/>
  <state_interface name="effort"/>
</joint>

But I'm looking for a solution to instantiate an angle the arm joints as soon as the robot spawns in gazebo whether the simulation is running or the controllers activated.

Has anyone any lead ?

I also tried to add a -J paramter in the roslaunch but it doesn't work.

  # Spawn Robot
    node_spawn_sacra = Node(package='ros_gz_sim', executable='create',
                arguments=[
                    '-name', LaunchConfiguration('robot_name'),
                    '-topic', '/robot_description',
                    '-x', LaunchConfiguration('robot_coord_X'),
                    '-z', LaunchConfiguration('robot_coord_Z'),
                    '-y', LaunchConfiguration('robot_coord_Y'),
                    '-R', LaunchConfiguration('robot_orien_R'),
                    '-P', LaunchConfiguration('robot_orien_P'),
                    '-Y', LaunchConfiguration('robot_orien_Y'),
                    '-J', 'joint1 0.5',
                    '-J', 'joint2 0.9'
                    ],
                output='screen',
                )
edit flag offensive delete link more

Comments

Which version of ROS2, gazebo and gazebo_ros2_control are you using? This feature was backported to galactic, but won't happen for foxy. https://github.com/ros-controls/gazeb... Similar issue here: https://github.com/UniversalRobots/Un...

christophfroehlich gravatar imagechristophfroehlich ( 2022-12-12 02:39:10 -0500 )edit
0

answered 2022-12-11 06:28:55 -0500

Read this discussion. May be this can help you here https://answers.gazebosim.org/question/27584/ros2-setting-russia gosloto results-initial-joint-position-at-spawn/. If yes then please mentioned me here.

edit flag offensive delete link more

Comments

The URL you provided seems broken.

Luczia gravatar imageLuczia ( 2023-05-29 19:39:03 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2021-11-17 10:45:18 -0500

Seen: 2,688 times

Last updated: Dec 09 '22