def generate_launch_description():
"""
Returns ROS2 LaunchDescription object.
"""
gzclient = True
real_speed = False
multi_instance = False
port = 11345
urdf = get_prefix_path(
"mara_description"
) + "/share/mara_description/urdf/mara_robot_gripper_140.urdf"
return ut_launch.generate_launch_description_mara(gzclient, real_speed,
multi_instance, port,
urdf)
def __init__(self):
"""
Initialize the MARA environemnt
"""
# Manage command line args
args = ut_generic.getArgsParserMARA().parse_args()
self.gzclient = args.gzclient
self.real_speed = args.real_speed
self.velocity = args.velocity
self.multi_instance = args.multi_instance
self.port = args.port
# Set the path of the corresponding URDF file
URDF_PATH = get_prefix_path(
"mara_description"
) + "/share/mara_description/urdf/mara_robot_gripper_140.urdf"
# Launch mara in a new Process
ut_launch.start_launch_servide_process(
ut_launch.generate_launch_description_mara(self.gzclient,
self.real_speed,
self.multi_instance,
self.port, URDF_PATH))
# Wait a bit for the spawn process.
# TODO, replace sleep function.
time.sleep(5)
# Create the node after the new ROS_DOMAIN_ID is set in generate_launch_description()
rclpy.init(args=None)
self.node = rclpy.create_node(self.__class__.__name__)
# class variables
self._observation_msg = None
self.obs = None
self.action_space = None
self.target_position = None
self.target_orientation = None
self.max_episode_steps = 1024
self.iterator = 0
self.reset_jnts = True
self._collision_msg = None
#############################
# Environment hyperparams
#############################
# Target, where should the agent reach
self.target_position = np.asarray([-0.40028, 0.095615,
0.72466]) # close to the table
self.target_orientation = np.asarray(
[0., 0.7071068, 0.7071068, 0.]) # arrow looking down [w, x, y, z]
# self.target_position = np.asarray([-0.386752, -0.000756, 1.40557]) # easy point
# self.target_orientation = np.asarray([-0.4958324, 0.5041332, 0.5041331, -0.4958324]) # arrow looking opposite to MARA [w, x, y, z]
EE_POINTS = np.asmatrix([[0, 0, 0]])
EE_VELOCITIES = np.asmatrix([[0, 0, 0]])
# Initial joint position
INITIAL_JOINTS = np.array([0., 0., 0., 0., 0., 0.])
# INITIAL_JOINTS = np.array([0., 0., 0., 0., -1.57, 0.])
# # Topics for the robot publisher and subscriber.
JOINT_PUBLISHER = '/mara_controller/command'
JOINT_SUBSCRIBER = '/mara_controller/state'
# joint names:
MOTOR1_JOINT = 'motor1'
MOTOR2_JOINT = 'motor2'
MOTOR3_JOINT = 'motor3'
MOTOR4_JOINT = 'motor4'
MOTOR5_JOINT = 'motor5'
MOTOR6_JOINT = 'motor6'
EE_LINK = 'ee_link'
# Set constants for links
WORLD = 'world'
TABLE = 'table'
BASE = 'base_link'
BASE_ROBOT = 'base_robot'
MARA_MOTOR1_LINK = 'motor1_link'
MARA_MOTOR2_LINK = 'motor2_link'
MARA_MOTOR3_LINK = 'motor3_link'
MARA_MOTOR4_LINK = 'motor4_link'
MARA_MOTOR5_LINK = 'motor5_link'
MARA_MOTOR6_LINK = 'motor6_link'
EE_LINK = 'ee_link'
JOINT_ORDER = [
MOTOR1_JOINT, MOTOR2_JOINT, MOTOR3_JOINT, MOTOR4_JOINT,
MOTOR5_JOINT, MOTOR6_JOINT
]
LINK_NAMES = [
WORLD, TABLE, BASE, BASE_ROBOT, MARA_MOTOR1_LINK, MARA_MOTOR2_LINK,
MARA_MOTOR3_LINK, MARA_MOTOR4_LINK, MARA_MOTOR5_LINK,
MARA_MOTOR6_LINK, EE_LINK
]
reset_condition = {
'initial_positions': INITIAL_JOINTS,
'initial_velocities': []
}
#############################
m_joint_order = copy.deepcopy(JOINT_ORDER)
m_link_names = copy.deepcopy(LINK_NAMES)
# Initialize target end effector position
self.environment = {
'joint_order': m_joint_order,
'link_names': m_link_names,
'reset_conditions': reset_condition,
'tree_path': URDF_PATH,
'end_effector_points': EE_POINTS,
}
# Subscribe to the appropriate topics, taking into account the particular robot
self._pub = self.node.create_publisher(
JointTrajectory,
JOINT_PUBLISHER,
qos_profile=qos_profile_sensor_data)
self._sub = self.node.create_subscription(
JointTrajectoryControllerState,
JOINT_SUBSCRIBER,
self.observation_callback,
qos_profile=qos_profile_sensor_data)
self._sub_coll = self.node.create_subscription(
ContactState,
'/gazebo_contacts',
self.collision_callback,
qos_profile=qos_profile_sensor_data)
self.reset_sim = self.node.create_client(Empty, '/reset_simulation')
# Initialize a tree structure from the robot urdf.
# note that the xacro of the urdf is updated by hand.
# The urdf must be compiled.
_, self.ur_tree = tree_urdf.treeFromFile(self.environment['tree_path'])
# Retrieve a chain structure between the base and the start of the end effector.
self.mara_chain = self.ur_tree.getChain(
self.environment['link_names'][0],
self.environment['link_names'][-1])
self.num_joints = self.mara_chain.getNrOfJoints()
# Initialize a KDL Jacobian solver from the chain.
self.jac_solver = ChainJntToJacSolver(self.mara_chain)
self.obs_dim = self.num_joints + 6
# # Here idially we should find the control range of the robot. Unfortunatelly in ROS/KDL there is nothing like this.
# # I have tested this with the mujoco enviroment and the output is always same low[-1.,-1.], high[1.,1.]
low = -np.pi * np.ones(self.num_joints)
high = np.pi * np.ones(self.num_joints)
self.action_space = spaces.Box(low, high)
high = np.inf * np.ones(self.obs_dim)
low = -high
self.observation_space = spaces.Box(low, high)
# Spawn Target element in gazebo.
# node & spawn_cli initialized in super class
spawn_cli = self.node.create_client(SpawnEntity, '/spawn_entity')
while not spawn_cli.wait_for_service(timeout_sec=1.0):
self.node.get_logger().info(
'service not available, waiting again...')
model_xml = ut_gazebo.get_target_sdf()
pose = Pose()
pose.position.x = self.target_position[0]
pose.position.y = self.target_position[1]
pose.position.z = self.target_position[2]
pose.orientation.x = self.target_orientation[1]
pose.orientation.y = self.target_orientation[2]
pose.orientation.z = self.target_orientation[3]
pose.orientation.w = self.target_orientation[0]
#override previous spawn_request element.
self.spawn_request = SpawnEntity.Request()
self.spawn_request.name = "target"
self.spawn_request.xml = model_xml
self.spawn_request.robot_namespace = ""
self.spawn_request.initial_pose = pose
self.spawn_request.reference_frame = "world"
#ROS2 Spawn Entity
target_future = spawn_cli.call_async(self.spawn_request)
rclpy.spin_until_future_complete(self.node, target_future)
# Seed the environment
self.seed()