def test_api(): package_names = get_package_names() assert isinstance(package_names, Iterable) # explicit dependencies of this package will for sure be available assert 'ros2cli' in package_names prefix_path = get_prefix_path('ros2cli') assert os.path.isdir(prefix_path) prefix_path = get_prefix_path('_not_existing_package_name') assert prefix_path is None
def test_api(): package_names = get_package_names() assert isinstance(package_names, Iterable) # explicit dependencies of this package will for sure be available known_package_names = ( 'ament_copyright', 'ament_flake8', 'ament_pep257', 'ros2cli') for known_package_name in known_package_names: assert known_package_name in package_names prefix_path = get_prefix_path(known_package_name) assert os.path.isdir(prefix_path) prefix_path = get_prefix_path('_not_existing_package_name') assert prefix_path is None
def main(args=None): rclpy.init(args=args) node = rclpy.create_node('minimal_client') cli = node.create_client(SpawnEntity, '/spawn_entity') prefix_path = get_prefix_path('hans_t30_description') assert os.path.isdir(prefix_path) content = "" with open(prefix_path + "/share/hans_t30_description/urdf/hans_t30.urdf", 'r') as content_file: content = content_file.read() req = SpawnEntity.Request() req.name = "hans_t30" req.xml = content req.robot_namespace = "" req.reference_frame = "world" 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()
def main(self, *, args): if not args.share: prefix_path = get_prefix_path(args.package_name) if prefix_path is None: return PACKAGE_NOT_FOUND print(prefix_path) else: try: print(get_package_share_directory(args.package_name)) except PackageNotFoundError: return PACKAGE_NOT_FOUND
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 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.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 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): """ Initialize the MARA environemnt """ # Manage command line args args = ut_generic.getParserArgsRobot().parse_args() self.gzclient = args.gzclient self.realSpeed = args.realSpeed # self.realSpeed = True self.debug = args.debug self.multiInstance = args.multiInstance self.port = args.port # Set the path of the corresponding URDF file if self.realSpeed: urdf = "biped.urdf" self.urdfPath = get_prefix_path( "lobot_description" ) + "/share/lobot_description/robots/" + urdf else: print("Non real speed not yet supported. Use real speed instead. ") # TODO: Include launch logic here, refer to code from the .launch.py files # Note that after including the launch logic the code will no longer be debuggable due to multi process stuff # Create the node after the new ROS_DOMAIN_ID is set in generate_launch_description() rclpy.init() 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 ############################# EE_POINTS = np.asmatrix([[0, 0, 0]]) EE_VELOCITIES = np.asmatrix([[0, 0, 0]]) # # Topics for the robot publisher and subscriber. JOINT_PUBLISHER = '/lobot_arm/control' # Get Joint names from the parameter server get_joints_client = self.node.create_client( GetAllJoints, "/GetAllControlJoints", qos_profile=qos_profile_services_default) req = GetAllJoints.Request() req.robot = "lobot_arm" while not get_joints_client.wait_for_service(timeout_sec=3.0): self.node.get_logger().info( 'service not available, waiting again...') future = get_joints_client.call_async(req) rclpy.spin_until_future_complete(self.node, future) if future.result() is not None: joint_names = future.result().joints self.node.get_logger().info('Number of joints: %d' % (len(joint_names))) else: self.node.get_logger().info('Service call failed %r' % (future.exception(), )) JOINT_ORDER = joint_names INITIAL_JOINTS = np.full((len(joint_names)), 0.0).tolist() reset_condition = { 'initial_positions': INITIAL_JOINTS, 'initial_velocities': [] } ############################# m_jointOrder = copy.deepcopy(JOINT_ORDER) # Initialize target end effector position self.environment = { 'jointOrder': m_jointOrder, 'reset_conditions': reset_condition, 'tree_path': self.urdfPath, 'end_effector_points': EE_POINTS, } # Subscribe to the appropriate topics, taking into account the particular robot self._pub = self.node.create_publisher( JointControl, JOINT_PUBLISHER, qos_profile=qos_profile_sensor_data) self._sub = self.node.create_subscription(JointState, "/joint_states", self.observation_callback, qos_profile_sensor_data) # TODO: Make the clock node run on a separate thread so weird issues like outdated clock can stop happening self.lock = threading.Lock() self.clock_node = rclpy.create_node(self.__class__.__name__ + "_clock") self._sub_clock = self.clock_node.create_subscription( RosClock, '/clock', self.clock_callback, qos_profile=qos_profile_sensor_data) self.exec = rclpy.executors.MultiThreadedExecutor() self.exec.add_node(self.clock_node) t1 = threading.Thread(target=self.spinClockNode, daemon=True) t1.start() # self._imu_sub = self.node.create_subscription(JointState, "/lobot_IMU_controller/out", self.imu_callback, qos_profile_sensor_data) # self._sub = self.node.create_subscription(JointTrajectoryControllerState, JOINT_SUBSCRIBER, self.observation_callback, qos_profile=qos_profile_sensor_data) self._reset_sim = self.node.create_client(Empty, '/reset_simulation') self._physics_pauser = self.node.create_client(Empty, '/pause_physics') self._robot_resetter = self.node.create_client(Empty, '/lobot_arm/reset') self._physics_unpauser = self.node.create_client( Empty, '/unpause_physics') self.delete_entity = self.node.create_client(DeleteEntity, '/delete_entity') self.numJoints = len(JOINT_ORDER) # Initialize a KDL Jacobian solver from the chain. # self.jacSolver = ChainJntToJacSolver(self.mara_chain) # Observable dimensions, each joint has 2 (joint position + joint velocity), the IMU gives 6 self.obs_dim = self.numJoints * 2 + 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) * 0.4 high = np.pi * np.ones(self.numJoints) * 0.4 self.action_space = spaces.Box(low, high) high = np.inf * np.ones(self.obs_dim) low = -high self.observation_space = spaces.Box(low, high) self.seed() self.buffer_dist_rewards = [] self.buffer_tot_rewards = [] self.collided = 0 # Set the time source self._sim_time = 0 self._sim_time_msg = builtin_interfaces.msg.Time()
def main(self, *, args): prefix_path = get_prefix_path(args.package_name) if prefix_path is None: return 'Package not found' print(prefix_path)
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): 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 = []