def calculate_joints(self, position):
self.joint_angle_matricies = []
for angle in position:
self.joint_angle_matricies.append(
tf.transformations.euler_matrix(0, 0, angle))
T_c = [np.identity(4)]
link_states = LinkStates()
for index in xrange(len(self.urdf_tranform_matricies)):
urdf_transform_matrix = self.urdf_tranform_matricies[index]
joint_angle_matrix = self.joint_angle_matricies[index]
transform_matrix = np.dot(urdf_transform_matrix,
joint_angle_matrix)
if index in (6, 7, 8): #sets parent of fingers to link6
index = 6
elif index in (9, 10, 11): #sets parent of fingertip
index -= 2
T_c.append(np.dot(T_c[index], transform_matrix))
link_states.pose.append(msgify(Pose, T_c[-1]))
link_states.name.append(self.urdf_transforms[index].child_frame_id)
#print transforms.header
#print link_states.name[-1]
#print link_states.pose[-1]
#print '-----------------------------------------------'
self.link_states = link_states
def __init__(self):
# Only variable needed to be set here
self.action_space = spaces.Discrete(6)
# This is the most common case of Box observation type
high = np.full((18), np.inf)
self.observation_space = spaces.Box(-high, high)
# Variables that we retrieve through the param server, loded when launch training launch.
self.action = 0
self.init_positions = [
0.0, 2.9, 1.3, -2.07, 1.4, 0.0, 1, 1, 1, 0.0, 0.0, 0.0
]
self.joint_angles = [
0.0, 2.9, 1.3, -2.07, 1.4, 0.0, 1, 1, 1, 0.0, 0.0, 0.0
]
self.target_point = [0.5, 0.5, 0.5]
self.joint_pos_increment_value = 0.1
self.n_step = 0
self.n_episode = 0
self.joint_names = [
'j2n6s300_joint_1', 'j2n6s300_joint_2', 'j2n6s300_joint_3',
'j2n6s300_joint_4', 'j2n6s300_joint_5', 'j2n6s300_joint_6',
'j2n6s300_joint_finger_1', 'j2n6s300_joint_finger_2',
'j2n6s300_joint_finger_3', 'j2n6s300_joint_finger_tip_1',
'j2n6s300_joint_finger_tip_2', 'j2n6s300_joint_finger_tip_3'
]
self.frame_list = [
'root',
'j2n6s300_link_1',
'j2n6s300_link_2',
'j2n6s300_link_3',
'j2n6s300_link_4',
'j2n6s300_link_5',
'j2n6s300_link_6', #1-6
'j2n6s300_link_finger_1',
'j2n6s300_link_finger_2',
'j2n6s300_link_finger_3', #7-9
'j2n6s300_link_finger_tip_1',
'j2n6s300_link_finger_tip_2',
'j2n6s300_link_finger_tip_3',
'j2n6s300_end_effector'
] #10-12
self.counter = 0
self.timer = time.time()
self.pub = rospy.Publisher("joint_states", JointState, queue_size=10)
self.joint_state = JointState()
self.joint_state.name = self.joint_names
self.link_states = LinkStates()
parser = UrdfParser()
self.urdf_tranform_matricies = parser.get_urdf_trans_mats()
self.urdf_transforms = parser.get_urdf_trans()
self.calculate_joints(self.init_positions)
self.reset()
def __init__(self):
self.cubes_index = []
self.cubes_pose = LinkStates()
self.states_sub = rospy.Subscriber("/gazebo/link_states", LinkStates,
self.callback)
self.pose_pub = rospy.Publisher("/gazebo/cubes",
LinkStates,
queue_size=1)
def __init__(self):
self.pub_link = rospy.Publisher('/gazebo/set_link_state',
LinkState,
queue_size=10)
self.current_pose = Pose()
self.current_twist = Twist()
self.links = LinkStates()
self.j = 1
def get_tf(self):
rospy.logwarn(
"Publishing /j2n6s300/link_states. Works with real and simulated j2n6s300 robot"
)
pose = Pose()
twist = Twist
time.sleep(.7) #wait for listener buffer to fill
while not rospy.is_shutdown():
msg = LinkStates()
msg.name = self.frame_list
query_rostime = self.listener.getLatestCommonTime(
'root', 'j2n6s300_link_finger_tip_3')
try:
for name in self.frame_list:
i = self.frame_list.index(name)
(tran, rot) = self.listener.lookupTransform(
'root', name, query_rostime)
(tran2, rot2) = self.listener.lookupTransform(
'root', name,
query_rostime - rospy.Duration.from_sec(self.delta_t))
msg.pose.append(Pose())
msg.twist.append(Twist())
msg.pose[i].position.x = tran[0]
msg.pose[i].position.y = tran[1]
msg.pose[i].position.z = tran[2]
msg.pose[i].orientation.x = rot[0]
msg.pose[i].orientation.y = rot[1]
msg.pose[i].orientation.z = rot[2]
msg.pose[i].orientation.w = rot[3]
msg.twist[i].linear.x = (tran[0] - tran2[0]) / self.delta_t
msg.twist[i].linear.y = (tran[1] - tran2[1]) / self.delta_t
msg.twist[i].linear.z = (tran[2] - tran2[2]) / self.delta_t
#distance = math.sqrt((tran[0]-tran2[0])**2+(tran[1]-tran2[1])**2+(tran[2]-tran2[2])**2)
#vel = float(distance/.5)
except Exception as e:
rospy.logerr(e)
time.sleep(1)
continue
self.twist_pub.publish(msg)
rospy.Rate(25).sleep()
rospy.spin()
def topic_callback(self):
T_c = [np.identity(4)]
link_states = LinkStates()
for index in xrange(len(self.urdf_tranform_matricies)):
urdf_transform_matrix = self.urdf_tranform_matricies[index]
joint_angle_matrix = self.joint_angle_matricies[index]
transform_matrix = np.dot(urdf_transform_matrix,
joint_angle_matrix)
if index in (6, 7, 8): #sets parent of fingers to link6
index = 6
elif index in (9, 10, 11): #sets parent of fingertip
index -= 2
T_c.append(np.dot(T_c[index], transform_matrix))
link_states.pose.append(msgify(Pose, T_c[-1]))
link_states.name.append(self.urdf_transforms[index].child_frame_id)
#transforms.header.frame_id = transforms.child_frame_id
#print transforms.header
#print link_states.name[-1]
#print link_states.pose[-1]
#print '-----------------------------------------------'
self.pub.publish(link_states)
def __init__(self):
rospy.logdebug("Starting URSimDoorOpening Class object...")
# Init GAZEBO Objects
self.set_obj_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
self.get_world_state = rospy.ServiceProxy('/gazebo/get_world_properties', GetWorldProperties)
# Subscribe joint state and target pose
rospy.Subscriber("/ft_sensor_topic", WrenchStamped, self.wrench_stamped_callback)
rospy.Subscriber("/joint_states", JointState, self.joints_state_callback)
rospy.Subscriber("/gazebo/link_states", LinkStates, self.link_state_callback)
# Gets training parameters from param server
self.desired_pose = Pose()
self.running_step = rospy.get_param("/running_step")
self.max_height = rospy.get_param("/max_height")
self.min_height = rospy.get_param("/min_height")
self.observations = rospy.get_param("/observations")
# Joint limitation
shp_max = rospy.get_param("/joint_limits_array/shp_max")
shp_min = rospy.get_param("/joint_limits_array/shp_min")
shl_max = rospy.get_param("/joint_limits_array/shl_max")
shl_min = rospy.get_param("/joint_limits_array/shl_min")
elb_max = rospy.get_param("/joint_limits_array/elb_max")
elb_min = rospy.get_param("/joint_limits_array/elb_min")
wr1_max = rospy.get_param("/joint_limits_array/wr1_max")
wr1_min = rospy.get_param("/joint_limits_array/wr1_min")
wr2_max = rospy.get_param("/joint_limits_array/wr2_max")
wr2_min = rospy.get_param("/joint_limits_array/wr2_min")
wr3_max = rospy.get_param("/joint_limits_array/wr3_max")
wr3_min = rospy.get_param("/joint_limits_array/wr3_min")
self.joint_limits = {"shp_max": shp_max,
"shp_min": shp_min,
"shl_max": shl_max,
"shl_min": shl_min,
"elb_max": elb_max,
"elb_min": elb_min,
"wr1_max": wr1_max,
"wr1_min": wr1_min,
"wr2_max": wr2_max,
"wr2_min": wr2_min,
"wr3_max": wr3_max,
"wr3_min": wr3_min
}
shp_init_value0 = rospy.get_param("/init_joint_pose0/shp")
shl_init_value0 = rospy.get_param("/init_joint_pose0/shl")
elb_init_value0 = rospy.get_param("/init_joint_pose0/elb")
wr1_init_value0 = rospy.get_param("/init_joint_pose0/wr1")
wr2_init_value0 = rospy.get_param("/init_joint_pose0/wr2")
wr3_init_value0 = rospy.get_param("/init_joint_pose0/wr3")
self.init_joint_pose0 = [shp_init_value0, shl_init_value0, elb_init_value0, wr1_init_value0, wr2_init_value0, wr3_init_value0]
shp_init_value1 = rospy.get_param("/init_joint_pose1/shp")
shl_init_value1 = rospy.get_param("/init_joint_pose1/shl")
elb_init_value1 = rospy.get_param("/init_joint_pose1/elb")
wr1_init_value1 = rospy.get_param("/init_joint_pose1/wr1")
wr2_init_value1 = rospy.get_param("/init_joint_pose1/wr2")
wr3_init_value1 = rospy.get_param("/init_joint_pose1/wr3")
self.init_joint_pose1 = [shp_init_value1, shl_init_value1, elb_init_value1, wr1_init_value1, wr2_init_value1, wr3_init_value1]
# print("[init_joint_pose1]: ", [shp_init_value1, shl_init_value1, elb_init_value1, wr1_init_value1, wr2_init_value1, wr3_init_value1])
shp_init_value2 = rospy.get_param("/init_joint_pose2/shp")
shl_init_value2 = rospy.get_param("/init_joint_pose2/shl")
elb_init_value2 = rospy.get_param("/init_joint_pose2/elb")
wr1_init_value2 = rospy.get_param("/init_joint_pose2/wr1")
wr2_init_value2 = rospy.get_param("/init_joint_pose2/wr2")
wr3_init_value2 = rospy.get_param("/init_joint_pose2/wr3")
self.init_joint_pose2 = [shp_init_value2, shl_init_value2, elb_init_value2, wr1_init_value2, wr2_init_value2, wr3_init_value2]
# print("[init_joint_pose2]: ", [shp_init_value2, shl_init_value2, elb_init_value2, wr1_init_value2, wr2_init_value2, wr3_init_value2])
shp_after_rotate = rospy.get_param("/eelink_pose_after_rotate/shp")
shl_after_rotate = rospy.get_param("/eelink_pose_after_rotate/shl")
elb_after_rotate = rospy.get_param("/eelink_pose_after_rotate/elb")
wr1_after_rotate = rospy.get_param("/eelink_pose_after_rotate/wr1")
wr2_after_rotate = rospy.get_param("/eelink_pose_after_rotate/wr2")
wr3_after_rotate = rospy.get_param("/eelink_pose_after_rotate/wr3")
self.after_rotate = [shp_after_rotate, shl_after_rotate, elb_after_rotate, wr1_after_rotate, wr2_after_rotate, wr3_after_rotate]
shp_after_pull = rospy.get_param("/eelink_pose_after_pull/shp")
shl_after_pull = rospy.get_param("/eelink_pose_after_pull/shl")
elb_after_pull = rospy.get_param("/eelink_pose_after_pull/elb")
wr1_after_pull = rospy.get_param("/eelink_pose_after_pull/wr1")
wr2_after_pull = rospy.get_param("/eelink_pose_after_pull/wr2")
wr3_after_pull = rospy.get_param("/eelink_pose_after_pull/wr3")
self.after_pull = [shp_after_pull, shl_after_pull, elb_after_pull, wr1_after_pull, wr2_after_pull, wr3_after_pull]
r_drv_value1 = rospy.get_param("/init_grp_pose1/r_drive")
l_drv_value1 = rospy.get_param("/init_grp_pose1/l_drive")
r_flw_value1 = rospy.get_param("/init_grp_pose1/r_follower")
l_flw_value1 = rospy.get_param("/init_grp_pose1/l_follower")
r_spr_value1 = rospy.get_param("/init_grp_pose1/r_spring")
l_spr_value1 = rospy.get_param("/init_grp_pose1/l_spring")
r_drv_value2 = rospy.get_param("/init_grp_pose2/r_drive")
l_drv_value2 = rospy.get_param("/init_grp_pose2/l_drive")
r_flw_value2 = rospy.get_param("/init_grp_pose2/r_follower")
l_flw_value2 = rospy.get_param("/init_grp_pose2/l_follower")
r_spr_value2 = rospy.get_param("/init_grp_pose2/r_spring")
l_spr_value2 = rospy.get_param("/init_grp_pose2/l_spring")
self.init_grp_pose1 = [r_drv_value1, l_drv_value1, r_flw_value1, l_flw_value1, r_spr_value1, l_spr_value1]
self.init_grp_pose2 = [r_drv_value2, l_drv_value2, r_flw_value2, l_flw_value2, r_spr_value2, l_spr_value2]
# Fill in the Done Episode Criteria list
self.episode_done_criteria = rospy.get_param("/episode_done_criteria")
# stablishes connection with simulator
self._gz_conn = GazeboConnection()
self._ctrl_conn = ControllersConnection(namespace="")
# Controller type for ros_control
self._ctrl_type = rospy.get_param("/control_type")
self.pre_ctrl_type = self._ctrl_type
# Get the force and troque limit
self.force_limit = rospy.get_param("/force_limit")
self.torque_limit = rospy.get_param("/torque_limit")
# We init the observations
self.base_orientation = Quaternion()
self.imu_link = Quaternion()
self.door = Quaternion()
self.quat = Quaternion()
self.imu_link_rpy = Vector3()
self.door_rpy = Vector3()
self.link_state = LinkStates()
self.wrench_stamped = WrenchStamped()
self.joints_state = JointState()
self.end_effector = Point()
self.previous_action =[]
self.counter = 0
self.max_rewards = 1
# Arm/Control parameters
self._ik_params = setups['UR5_6dof']['ik_params']
# ROS msg type
self._joint_pubisher = JointPub()
self._joint_traj_pubisher = JointTrajPub()
# Gym interface and action
self.action_space = spaces.Discrete(6)
self.observation_space = 21 #np.arange(self.get_observations().shape[0])
## self.observation_space = 15 #np.arange(self.get_observations().shape[0])
self.reward_range = (-np.inf, np.inf)
self._seed()
# Change the controller type
set_joint_pos_server = rospy.Service('/set_position_controller', SetBool, self._set_pos_ctrl)
set_joint_traj_pos_server = rospy.Service('/set_trajectory_position_controller', SetBool, self._set_traj_pos_ctrl)
set_joint_vel_server = rospy.Service('/set_velocity_controller', SetBool, self._set_vel_ctrl)
set_joint_traj_vel_server = rospy.Service('/set_trajectory_velocity_controller', SetBool, self._set_traj_vel_ctrl)
# set_gripper_server = rospy.Service('/set_gripper_controller', SetBool, self._set_grp_ctrl)
self.pos_traj_controller = ['joint_state_controller',
'gripper_controller',
'pos_traj_controller']
self.pos_controller = ["joint_state_controller",
"gripper_controller",
"ur_shoulder_pan_pos_controller",
"ur_shoulder_lift_pos_controller",
"ur_elbow_pos_controller",
"ur_wrist_1_pos_controller",
"ur_wrist_2_pos_controller",
"ur_wrist_3_pos_controller"]
self.vel_traj_controller = ['joint_state_controller',
'gripper_controller',
'vel_traj_controller']
self.vel_controller = ["joint_state_controller",
"gripper_controller",
"ur_shoulder_pan_vel_controller",
"ur_shoulder_lift_vel_controller",
"ur_elbow_vel_controller",
"ur_wrist_1_vel_controller",
"ur_wrist_2_vel_controller",
"ur_wrist_3_vel_controller"]
# Helpful False
self.stop_flag = False
stop_trainning_server = rospy.Service('/stop_training', SetBool, self._stop_trainnig)
start_trainning_server = rospy.Service('/start_training', SetBool, self._start_trainnig)
def rosPublish(self):
'''
Publish topics at standard frequency
'''
robot_models_msg = ModelStates()
for robot_id in self._gazebo_robots:
if self._gazebo_robots[robot_id]['model'].is_received():
state = self._gazebo_robots[robot_id]['model'].get()
robot_models_msg.name.append(state.model_name)
robot_models_msg.pose.append(state.pose)
robot_models_msg.twist.append(state.twist)
self._gazebo_robot_models_publisher.publish(robot_models_msg)
picking_models_msg = ModelStates()
for picking_id in self._gazebo_objects:
if self._gazebo_objects[picking_id]['model'].is_received():
state = self._gazebo_objects[picking_id]['model'].get()
picking_models_msg.name.append(state.model_name)
picking_models_msg.pose.append(state.pose)
picking_models_msg.twist.append(state.twist)
self._gazebo_object_models_publisher.publish(picking_models_msg)
robot_links_msg = LinkStates()
for robot_id in self._gazebo_robots:
for link_id in self._gazebo_robots[robot_id]['links']:
if self._gazebo_robots[robot_id]['links'][link_id].is_received(
):
state = self._gazebo_robots[robot_id]['links'][
link_id].get()
robot_links_msg.name.append(state.link_name)
robot_links_msg.pose.append(state.pose)
robot_links_msg.twist.append(state.twist)
self._gazebo_robot_links_publisher.publish(robot_links_msg)
picking_links_msg = LinkStates()
for picking_id in self._gazebo_objects:
for link_id in self._gazebo_objects[picking_id]['links']:
if self._gazebo_objects[picking_id]['links'][
link_id].is_received():
state = self._gazebo_objects[picking_id]['links'][
link_id].get()
picking_links_msg.name.append(state.link_name)
picking_links_msg.pose.append(state.pose)
picking_links_msg.twist.append(state.twist)
self._gazebo_object_links_publisher.publish(picking_links_msg)
#print self._gazebo_robots
#print self._gazebo_objects
return 0
def __init__(self):
rospy.logdebug("Starting URSimReaching Class object...")
# Init GAZEBO Objects
self.set_obj_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.get_world_state = rospy.ServiceProxy(
'/gazebo/get_world_properties', GetWorldProperties)
# Subscribe joint state and target pose
rospy.Subscriber("/joint_states", JointState,
self.joints_state_callback)
rospy.Subscriber("/target_blocks_pose", Point,
self.target_point_callback)
rospy.Subscriber("/gazebo/link_states", LinkStates,
self.link_state_callback)
# Gets training parameters from param server
self.desired_pose = Pose()
self.running_step = rospy.get_param("/running_step")
self.max_height = rospy.get_param("/max_height")
self.min_height = rospy.get_param("/min_height")
self.observations = rospy.get_param("/observations")
# Joint limitation
shp_max = rospy.get_param("/joint_limits_array/shp_max")
shp_min = rospy.get_param("/joint_limits_array/shp_min")
shl_max = rospy.get_param("/joint_limits_array/shl_max")
shl_min = rospy.get_param("/joint_limits_array/shl_min")
elb_max = rospy.get_param("/joint_limits_array/elb_max")
elb_min = rospy.get_param("/joint_limits_array/elb_min")
wr1_max = rospy.get_param("/joint_limits_array/wr1_max")
wr1_min = rospy.get_param("/joint_limits_array/wr1_min")
wr2_max = rospy.get_param("/joint_limits_array/wr2_max")
wr2_min = rospy.get_param("/joint_limits_array/wr2_min")
wr3_max = rospy.get_param("/joint_limits_array/wr3_max")
wr3_min = rospy.get_param("/joint_limits_array/wr3_min")
self.joint_limits = {
"shp_max": shp_max,
"shp_min": shp_min,
"shl_max": shl_max,
"shl_min": shl_min,
"elb_max": elb_max,
"elb_min": elb_min,
"wr1_max": wr1_max,
"wr1_min": wr1_min,
"wr2_max": wr2_max,
"wr2_min": wr2_min,
"wr3_max": wr3_max,
"wr3_min": wr3_min
}
shp_init_value = rospy.get_param("/init_joint_pose/shp")
shl_init_value = rospy.get_param("/init_joint_pose/shl")
elb_init_value = rospy.get_param("/init_joint_pose/elb")
wr1_init_value = rospy.get_param("/init_joint_pose/wr1")
wr2_init_value = rospy.get_param("/init_joint_pose/wr2")
wr3_init_value = rospy.get_param("/init_joint_pose/wr3")
self.init_joint_pose = [
shp_init_value, shl_init_value, elb_init_value, wr1_init_value,
wr2_init_value, wr3_init_value
]
# Fill in the Done Episode Criteria list
self.episode_done_criteria = rospy.get_param("/episode_done_criteria")
# stablishes connection with simulator
self._gz_conn = GazeboConnection()
self._ctrl_conn = ControllersConnection(namespace="")
# Controller type for ros_control
self._ctrl_type = rospy.get_param("/control_type")
self.pre_ctrl_type = self._ctrl_type
# We init the observations
self.base_orientation = Quaternion()
self.target_point = Point()
self.link_state = LinkStates()
self.joints_state = JointState()
self.end_effector = Point()
self.distance = 0.
# Arm/Control parameters
self._ik_params = setups['UR5_6dof']['ik_params']
# ROS msg type
self._joint_pubisher = JointPub()
self._joint_traj_pubisher = JointTrajPub()
# Gym interface and action
self.action_space = spaces.Discrete(6)
self.observation_space = 15 #np.arange(self.get_observations().shape[0])
self.reward_range = (-np.inf, np.inf)
self._seed()
# Change the controller type
set_joint_vel_server = rospy.Service('/set_velocity_controller',
SetBool, self._set_vel_ctrl)
set_joint_traj_vel_server = rospy.Service(
'/set_trajectory_velocity_controller', SetBool,
self._set_traj_vel_ctrl)
self.vel_traj_controller = [
'joint_state_controller', 'gripper_controller',
'vel_traj_controller'
]
self.vel_controller = [
"joint_state_controller", "gripper_controller",
"ur_shoulder_pan_vel_controller",
"ur_shoulder_lift_vel_controller", "ur_elbow_vel_controller",
"ur_wrist_1_vel_controller", "ur_wrist_2_vel_controller",
"ur_wrist_3_vel_controller"
]
# Helpful False
self.stop_flag = False
stop_trainning_server = rospy.Service('/stop_training', SetBool,
self._stop_trainnig)
start_trainning_server = rospy.Service('/start_training', SetBool,
self._start_trainnig)
def _ros_init(self):
# Can check log msgs according to log_level {rospy.DEBUG, rospy.INFO, rospy.WARN, rospy.ERROR}
rospy.init_node('RLkitUR', anonymous=True, log_level=rospy.DEBUG)
rospy.logdebug("Starting RLkitUR Class object...")
# Init GAZEBO Objects
self.set_obj_state = rospy.ServiceProxy('/gazebo/set_model_state',
SetModelState)
self.get_world_state = rospy.ServiceProxy(
'/gazebo/get_world_properties', GetWorldProperties)
# Subscribe joint state and target pose
rospy.Subscriber("/joint_states", JointState,
self.joints_state_callback)
rospy.Subscriber("/target_blocks_pose", Point,
self.target_point_callback)
rospy.Subscriber("/gazebo/link_states", LinkStates,
self.link_state_callback)
rospy.Subscriber("/collision_status", Bool, self.collision_status)
# Gets training parameters from param server
self.desired_pose = Pose()
self.running_step = rospy.get_param("/running_step")
self.max_height = rospy.get_param("/max_height")
self.min_height = rospy.get_param("/min_height")
self.observations = rospy.get_param("/observations")
self.joint_names = rospy.get_param("/joint_names")
# Joint limitation
shp_max = rospy.get_param("/joint_limits_array/shp_max")
shp_min = rospy.get_param("/joint_limits_array/shp_min")
shl_max = rospy.get_param("/joint_limits_array/shl_max")
shl_min = rospy.get_param("/joint_limits_array/shl_min")
elb_max = rospy.get_param("/joint_limits_array/elb_max")
elb_min = rospy.get_param("/joint_limits_array/elb_min")
wr1_max = rospy.get_param("/joint_limits_array/wr1_max")
wr1_min = rospy.get_param("/joint_limits_array/wr1_min")
wr2_max = rospy.get_param("/joint_limits_array/wr2_max")
wr2_min = rospy.get_param("/joint_limits_array/wr2_min")
wr3_max = rospy.get_param("/joint_limits_array/wr3_max")
wr3_min = rospy.get_param("/joint_limits_array/wr3_min")
self.joint_limits = {
"shp_max": shp_max,
"shp_min": shp_min,
"shl_max": shl_max,
"shl_min": shl_min,
"elb_max": elb_max,
"elb_min": elb_min,
"wr1_max": wr1_max,
"wr1_min": wr1_min,
"wr2_max": wr2_max,
"wr2_min": wr2_min,
"wr3_max": wr3_max,
"wr3_min": wr3_min
}
# Joint Velocity limitation
shp_vel_max = rospy.get_param("/joint_velocity_limits_array/shp_max")
shp_vel_min = rospy.get_param("/joint_velocity_limits_array/shp_min")
shl_vel_max = rospy.get_param("/joint_velocity_limits_array/shl_max")
shl_vel_min = rospy.get_param("/joint_velocity_limits_array/shl_min")
elb_vel_max = rospy.get_param("/joint_velocity_limits_array/elb_max")
elb_vel_min = rospy.get_param("/joint_velocity_limits_array/elb_min")
wr1_vel_max = rospy.get_param("/joint_velocity_limits_array/wr1_max")
wr1_vel_min = rospy.get_param("/joint_velocity_limits_array/wr1_min")
wr2_vel_max = rospy.get_param("/joint_velocity_limits_array/wr2_max")
wr2_vel_min = rospy.get_param("/joint_velocity_limits_array/wr2_min")
wr3_vel_max = rospy.get_param("/joint_velocity_limits_array/wr3_max")
wr3_vel_min = rospy.get_param("/joint_velocity_limits_array/wr3_min")
self.joint_velocty_limits = {
"shp_vel_max": shp_vel_max,
"shp_vel_min": shp_vel_min,
"shl_vel_max": shl_vel_max,
"shl_vel_min": shl_vel_min,
"elb_vel_max": elb_vel_max,
"elb_vel_min": elb_vel_min,
"wr1_vel_max": wr1_vel_max,
"wr1_vel_min": wr1_vel_min,
"wr2_vel_max": wr2_vel_max,
"wr2_vel_min": wr2_vel_min,
"wr3_vel_max": wr3_vel_max,
"wr3_vel_min": wr3_vel_min
}
# Init joint pose
shp_init_value = rospy.get_param("/init_joint_pose/shp")
shl_init_value = rospy.get_param("/init_joint_pose/shl")
elb_init_value = rospy.get_param("/init_joint_pose/elb")
wr1_init_value = rospy.get_param("/init_joint_pose/wr1")
wr2_init_value = rospy.get_param("/init_joint_pose/wr2")
wr3_init_value = rospy.get_param("/init_joint_pose/wr3")
self.init_joint_angles = [
shp_init_value, shl_init_value, elb_init_value, wr1_init_value,
wr2_init_value, wr3_init_value
]
# 3D coordinate limits
x_max = rospy.get_param("/cartesian_limits/x_max")
x_min = rospy.get_param("/cartesian_limits/x_min")
y_max = rospy.get_param("/cartesian_limits/y_max")
y_min = rospy.get_param("/cartesian_limits/y_min")
z_max = rospy.get_param("/cartesian_limits/z_max")
z_min = rospy.get_param("/cartesian_limits/z_min")
self.xyz_limits = {
"x_max": x_max,
"x_min": shp_vel_min,
"y_max": y_max,
"y_min": y_min,
"z_max": z_max,
"z_min": z_min
}
# Fill in the Done Episode Criteria list
self.episode_done_criteria = rospy.get_param("/episode_done_criteria")
# Stablishes connection with simulator
self._gz_conn = GazeboConnection()
self._ctrl_conn = ControllersConnection(namespace="")
# Controller type for ros_control
self.current_controller_type = rospy.get_param("/control_type")
self.pre_controller_type = self.current_controller_type
# Init the observations, target, ...
self.base_orientation = Quaternion()
self.target_point = Point()
self.link_state = LinkStates()
self.joints_state = JointState()
self.end_effector = Point()
self.distance = 0.
# Arm/Control parameters
self._ik_params = setups['UR5_6dof']['ik_params']
# ROS msg type
self._joint_pubisher = JointPub()
self._joint_traj_pubisher = JointTrajPub()
self.reset_publisher = rospy.Publisher("/ur/reset",
String,
queue_size=1)
self.target_point = Point()
self._target_point_pubisher = rospy.Publisher("/target_goal",
Point,
queue_size=1)
# Controller list
self.vel_traj_controller = [
'joint_state_controller', 'gripper_controller',
'vel_traj_controller'
]
self.pos_traj_controller = [
'joint_state_controller', 'gripper_controller',
'pos_traj_controller'
]
self.vel_controller = [
"joint_state_controller", "gripper_controller",
"ur_shoulder_pan_vel_controller",
"ur_shoulder_lift_vel_controller", "ur_elbow_vel_controller",
"ur_wrist_1_vel_controller", "ur_wrist_2_vel_controller",
"ur_wrist_3_vel_controller"
]
self.pos_controller = [
"joint_state_controller", "gripper_controller",
"ur_shoulder_pan_pos_controller",
"ur_shoulder_lift_pos_controller", "ur_elbow_pos_controller",
"ur_wrist_1_pos_controller", "ur_wrist_2_pos_controller",
"ur_wrist_3_pos_controller"
]
# Stop flag durning training
self.stop_flag = False