[ROS2] Setting initial joint position at spawn

edit

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".

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>