def generate_launch_description(): """ Returns ROS2 LaunchDescription object. """ gzclient = True realSpeed = False multiInstance = False port = 11345 urdf = "reinforcement_learning/mara_robot_gripper_140_train.urdf" return ut_launch.generateLaunchDescriptionMara(gzclient, realSpeed, multiInstance, port, urdf)
def generate_launch_description(): """ Returns ROS2 LaunchDescription object. """ gzclient = True realSpeed = False multiInstance = False port = 11345 urdf = "reinforcement_learning/mara_robot_gripper_140_train.urdf" urdfPath = get_prefix_path("mara_description") + "/share/mara_description/urdf/" + urdf return ut_launch.generateLaunchDescriptionMara(gzclient, realSpeed, multiInstance, port, urdfPath)
def __init__(self): """ Initialize the MARA environemnt """ # Manage command line args args = ut_generic.getArgsParserMARA().parse_args() self.gzclient = args.gzclient self.realSpeed = args.realSpeed self.velocity = args.velocity self.multiInstance = args.multiInstance self.port = args.port # Set the path of the corresponding URDF file if self.realSpeed: urdf = "reinforcement_learning/mara_robot_run.urdf" urdfPath = get_prefix_path( "mara_description") + "/share/mara_description/urdf/" + urdf else: urdf = "reinforcement_learning/mara_robot_train.urdf" urdfPath = get_prefix_path( "mara_description") + "/share/mara_description/urdf/" + urdf # Launch mara in a new Process self.launch_subp = ut_launch.startLaunchServiceProcess( ut_launch.generateLaunchDescriptionMara(self.gzclient, self.realSpeed, self.multiInstance, self.port, urdfPath)) # 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.max_episode_steps = 1024 #default value, can be updated from baselines self.iterator = 0 self.reset_jnts = True self._collision_msg = None ############################# # Environment hyperparams ############################# # Target, where should the agent reach self.targetPosition = 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.targetPosition = 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.]) # # 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' BASE = '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, BASE, 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_jointOrder = copy.deepcopy(JOINT_ORDER) m_linkNames = copy.deepcopy(LINK_NAMES) # Initialize target end effector position self.environment = { 'jointOrder': m_jointOrder, 'linkNames': m_linkNames, 'reset_conditions': reset_condition, 'tree_path': urdfPath, '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['linkNames'][0], self.environment['linkNames'][-1]) self.numJoints = self.mara_chain.getNrOfJoints() # Initialize a KDL Jacobian solver from the chain. self.jacSolver = ChainJntToJacSolver(self.mara_chain) self.obs_dim = self.numJoints + 10 # # 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.numJoints) high = np.pi * np.ones(self.numJoints) 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( '/spawn_entity service not available, waiting again...') modelXml = ut_gazebo.getTargetSdf() pose = Pose() pose.position.x = self.targetPosition[0] pose.position.y = self.targetPosition[1] pose.position.z = self.targetPosition[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 = modelXml 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() self.buffer_dist_rewards = [] self.buffer_tot_rewards = [] self.collided = 0
def __init__(self): """ Initialize the Delta MARA environemnt """ # Manage command line args args = ut_generic.getArgsParserMARA().parse_args() self.gzclient = args.gzclient self.realSpeed = args.realSpeed self.velocity = args.velocity self.multiInstance = args.multiInstance self.port = args.port # Set the path of the corresponding URDF file if self.realSpeed: urdfPath = get_prefix_path( "mara_description") + "/share/mara_description/urdf/" + urdf else: urdf = "reinforcement_learning/arc_delta_onTable.urdf" urdfPath = get_prefix_path( "mara_description") + "/share/mara_description/urdf/" + urdf # Launch mara in a new Process self.launch_subp = ut_launch.startLaunchServiceProcess( ut_launch.generateLaunchDescriptionMara(self.gzclient, self.realSpeed, self.multiInstance, self.port, urdfPath)) # 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.max_episode_steps = 1024 #default value, can be updated from baselines self.iterator = 0 self.reset_jnts = True self._collision_msg = None # Initial joint position INITIAL_JOINTS = np.array([0., 0., 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' # Set constants for links WORLD = 'world' BASE = 'link_0' #Added BASE MARA_MOTOR1_LINK = 'uleg_1' MARA_MOTOR2_LINK = 'uleg_2' MARA_MOTOR3_LINK = 'uleg_3' JOINT_ORDER = [MOTOR1_JOINT, MOTOR2_JOINT, MOTOR3_JOINT] LINK_NAMES = [ WORLD, BASE, MARA_MOTOR1_LINK, MARA_MOTOR2_LINK, MARA_MOTOR3_LINK ] reset_condition = { 'initial_positions': INITIAL_JOINTS, 'initial_velocities': [] } ############################# m_jointOrder = copy.deepcopy(JOINT_ORDER) m_linkNames = copy.deepcopy(LINK_NAMES) # Initialize target end effector position self.environment = { 'jointOrder': m_jointOrder, 'linkNames': m_linkNames, 'reset_conditions': reset_condition, } self.cli = self.node.create_client( ControlFinger, '/hrim_actuation_gripper_000000000004/goal') # 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') self.numJoints = 3 self.obs_dim = self.numJoints low = -np.pi * np.ones(self.numJoints) high = np.pi * np.ones(self.numJoints) 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( '/spawn_entity service not available, waiting again...')