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()
