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...')
def __init__(self): """ Initialize the MARA environemnt """ # Manage command line args args = ut_generic.getArgsParserMARA().parse_args() self.realSpeed = args.realSpeed self.velocity = args.velocity # Set the path of the corresponding URDF file urdfPath = get_prefix_path( "mara_description" ) + "/share/mara_description/urdf/reinforcement_learning/mara_robot_gripper_140_camera_run.urdf" # Launch mara in a new Process self.launch_subp = ut_launch.startLaunchServiceProcess( ut_launch.launchReal()) # 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 ############################# # 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) # 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 + 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.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) # Seed the environment self.seed()
def __init__(self): self.info = Info(self) """ Initialize the MARA environemnt """ # Manage command line args args = ut_generic.getArgsParserMARA().parse_args() self.gzclient = args.gzclient self.realSpeed = args.realSpeed #self.realSpeed = True self.velocity = args.velocity self.multiInstance = args.multiInstance self.port = args.port self.leg_name = "main" # Set the path of the corresponding URDF file if self.realSpeed: urdf = "phantomx.urdf" else: urdf = "phantomx.urdf" urdfPath = get_prefix_path( "mara_description") + "/share/mara_description/urdf/" + urdf # Launch phantomx in a new Process self.launch_subp = ut_launch_phantomx.startLaunchServiceProcess( ut_launch_phantomx.generateLaunchDescriptionPhantomX( self.gzclient, self.realSpeed, self.multiInstance, self.port, urdf)) # 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.max_episode_steps = 1024 self.iterator = 0 self.reset_jnts = True self.ground_truth = None self.limb_odoms = {} self.max_torque = 2.8 self.num_legs = 6 low = np.reshape( [-10.0, -10.0, -10.0] * 3 * 6 + [0.0] * 6 # + [-10, -10, -10] * 3 * 6 # + [-10, -10, -10] * 3 * 6 + [-2.0] * 6 # + [-10] * 1 + [-10] * 1 + [0] * 1 + # + [-10] * 6 # + [-2.8] * 18 #+ [-10.0, -10.0, -10.0] * 3 * 6 # + [0] , -1) high = np.reshape( [10.0, 10.0, 10.0] * 3 * 6 + [1.0] * 6 # + [10, 10, 10] * 3 * 6 # + [10, 10, 10] * 3 * 6 + [2.0] * 6 # + [10] * 1 + [10] * 1 + [2] * 1 + # + [10] * 6 # + [2.8] * 18 #+ [10.0, 10.0, 10.0] * 3 * 6 # + [1] , -1) self.observation_space = spaces.Box(low, high) low = -self.max_torque * np.ones(self.num_legs * 3) high = self.max_torque * np.ones(self.num_legs * 3) self.action_space = spaces.Box(low, high) # low = -np.pi * np.ones(3*2) # high = np.pi * np.ones(3*2) # self.observation_space = spaces.Box(low, high) # low = -np.pi * np.ones(6) # high = np.pi * np.ones(6) # self.action_space = spaces.Box(low, high) self._contact_msgs = {} self._collision = False self._ground_truth = Odometry() self._goal_height = 0.13 self.old_x = 0.0 self._pubs = {} self.limb_odom_sc = {} self.contact_sc = {} self.legs = ['lf', 'lm', 'lr', 'rf', 'rm', 'rr'] self.limbs = ['c1', 'thigh', 'tibia'] for leg in self.legs: self.contact_sc["tibia_" + leg] = self.node.create_subscription( ContactsState, '/tibia_' + leg + '_collision', self.leg_contact_callback) self._contact_msgs["tibia_" + leg] = 0 for limb in self.limbs: self._pubs[leg + "_" + limb] = self.node.create_publisher( Float32, 'j_' + limb + '_' + leg + '/force') self.limb_odoms[leg + "_" + limb] = None self.limb_odom_sc[leg + "_" + limb] = self.node.create_subscription( Odometry, '/' + leg + "_" + limb + '/odom', self.limb_odom_callback) self._ground_truth_sub = self.node.create_subscription( Odometry, '/odom', self.ground_truth_callback) self._body_ground_contact_sub = self.node.create_subscription( ContactsState, '/body_collision', self.body_contact_callback) self.reset_sim = self.node.create_client(Empty, '/reset_simulation') self.pause_sim = self.node.create_client(Empty, '/pause_physics') self.unpause_sim = self.node.create_client(Empty, '/unpause_physics') # Seed the environment self._body_ground_contact = 0 self.seed() self.buffer_dist_rewards = [] self.buffer_tot_rewards = []