[ROS2] Setting initial joint position at spawn
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 :
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
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>