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 = []
