def get_lsq_triangulation_error_with_noisy_gt(self,
observation,
epsilon=0.5):
noisy_joints = self.get_noisy_joints()
noisy_joints_neighbor = self.get_noisy_joints_neighbor()
self.joints = np.array(noisy_joints.res).reshape((14, 2))
self.joints_neighbor = np.array(noisy_joints_neighbor.res).reshape(
(14, 2))
# Get the camera intrinsics of both cameras
P1 = self.get_cam_intrinsic()
P2 = self.get_neighbor_cam_intrinsic()
# Convert world coordinates to local camera coordinates for both cameras
# We get the camera extrinsics as follows
trans, rot = self.listener.lookupTransform(
self.rotors_machine_name + '/xtion_rgb_optical_frame', 'world',
rospy.Time(0)) #target to source frame
(r, p, y) = tf.transformations.euler_from_quaternion(rot)
H1 = tf.transformations.euler_matrix(r, p, y, axes='sxyz')
H1[0:3, 3] = trans
print(
str(self.env_id) + ' ' + str(self.rotors_machine_name) + ':' +
str(trans))
trans, rot = self.listener.lookupTransform(
self.rotors_neighbor_name + '/xtion_rgb_optical_frame', 'world',
rospy.Time(0)) #target to source frame
(r, p, y) = tf.transformations.euler_from_quaternion(rot)
H2 = tf.transformations.euler_matrix(r, p, y, axes='sxyz')
H2[0:3, 3] = trans
print(
str(self.env_id) + ' ' + str(self.rotors_neighbor_name) + ':' +
str(trans))
#Concatenate Intrinsic Matrices
intrinsic = np.array((P1[0:3, 0:3], P2[0:3, 0:3]))
#Concatenate Extrinsic Matrices
extrinsic = np.array((H1, H2))
joints_gt = self.get_joints_gt()
error = 0
self.triangulated_root = PoseArray()
for k in range(len(gt_joints)):
p2d1 = self.joints[k, :]
p2d2 = self.joints_neighbor[k, :]
points_2d = np.array((p2d1, p2d2))
# Solve system of equations using least squares to estimate person position in robot 1 camera frame
estimate_root = self.lstsq_triangulation(intrinsic, extrinsic,
points_2d, 2)
es_root_cam = Point()
es_root_cam.x = estimate_root[0]
es_root_cam.y = estimate_root[1]
es_root_cam.z = estimate_root[2]
err_cov = PoseWithCovarianceStamped()
#if v>4: #because head, eyes and ears lead to high error for the actor
# if v==5 or v == 6:
p = Pose()
p.position = es_root_cam
self.triangulated_root.poses.append(p)
gt = joints_gt.poses[gt_joints[gt_joints_names[k]]].position
error += (self.get_distance_from_point(gt, es_root_cam))
err_cov.pose.pose.position = es_root_cam
err_cov.pose.covariance[0] = (gt.x - es_root_cam.x)**2
err_cov.pose.covariance[7] = (gt.y - es_root_cam.y)**2
err_cov.pose.covariance[14] = (gt.z - es_root_cam.z)**2
err_cov.header.stamp = rospy.Time()
err_cov.header.frame_id = 'world'
self.triangulated_cov_pub[k].publish(err_cov)
# Publish all estimates
self.triangulated_root.header.stamp = rospy.Time()
self.triangulated_root.header.frame_id = 'world'
self.triangulated_pub.publish(self.triangulated_root)
is_in_desired_pos = False
error = error / len(gt_joints)
if error <= epsilon:
# error = 0.4*error/epsilon
is_in_desired_pos = True
# else:
# error = 0.4
return [is_in_desired_pos, error]
def Initialize(self, parameters):
WalkingMode.Initialize(self, parameters)
self._LPP.Initialize()
self._command = 0
self._bRobotIsStatic = True
self._bGotStaticPose = False
self._BDI_Static_pose = Pose()
self._started_to_walk = False
self._target_pose = None
self._StepIndex = 1
self._isDynamic = False
self._stepWidth = 0.25 # Width of stride
self._theta_max = 0.30 # max turning angle per step
self._x_length_max = 0.25 # [meters] max step length (radius =~ 0.42 [m])
self._y_length_max = 0.15 # [meters]
# parameters to tune (see also 'Motion' task parameters):
self._err_rot = 0.018 #0.10 # [rad]
self._err_trans = 0.02 # 0.1 # [meters]
## USING task parameters:
if ((None != parameters) and ('Motion' in parameters)):
DesiredMotion = parameters['Motion']
if "Dynamic" == DesiredMotion: # Dynamic parameters
self._isDynamic = True
self._stepWidth = 0.2 #0.3 # Width of stride
self._theta_max = 0.15 #0.35 # max turning angle per step
self._x_length_max = 0.02 #0.25 # [meters] max step length (radius =~ 0.42 [m])
self._y_length_max = 0.15 #0.2 # [meters]
if "Dynamic10" == DesiredMotion: # Dynamic parameters
self._isDynamic = True
self._stepWidth = 0.2 #0.3 # Width of stride
self._theta_max = 0.15 #0.35 # max turning angle per step
self._x_length_max = 0.10 #0.25 # [meters] max step length (radius =~ 0.42 [m])
self._y_length_max = 0.10 #0.2 # [meters]
else: # Quasi-Static parameters (default)
self._isDynamic = False
self._stepWidth = 0.25 # Width of stride
self._theta_max = 0.30 # max turning angle per step
self._x_length_max = 0.25 # [meters] max step length (radius =~ 0.42 [m])
self._y_length_max = 0.15 # [meters]
self._R = self._stepWidth / 2 # Radius of turn, turn in place
# parameter 'Object' uses service to get delta alignment to target object
if ((None != parameters) and ('Object' in parameters)):
self._DesiredObject = parameters['Object']
else:
self._DesiredObject = "delta"
# parameter to turn in place and move (translate):
self._delta_yaw = 0.0
self._delta_trans = Point()
self._delta_trans.x = 0.0
self._delta_trans.y = 0.0
self._delta_trans.z = 0.0
# 'turn_in_place_Yaw' uses the parameter of the Yaw angle to turn in place
if ((None != parameters) and ('turn_in_place_Yaw' in parameters)):
self._target_pose = "Rotate_Translate_delta"
self._delta_yaw = float(parameters['turn_in_place_Yaw'])
# 'Xmovement' uses the parameter to translate x meters (+=fwd/-=bwd) from the starting place of the robot
if ((None != parameters) and ('Xmovement' in parameters)):
self._target_pose = "Rotate_Translate_delta"
self._delta_trans.x = float(parameters['Xmovement'])
# 'Ymovement' uses the parameter to translate y meters (+=left/-=right) from the starting place of the robot
if ((None != parameters) and ('Ymovement' in parameters)):
self._target_pose = "Rotate_Translate_delta"
self._delta_trans.y = float(parameters['Ymovement'])
if None == self._target_pose:
rospy.wait_for_service('C23/C66')
self._foot_placement_client = rospy.ServiceProxy(
'C23/C66', C23_orient) # object recognition service
# rospy.wait_for_service('foot_aline_pose')
# self._foot_placement_client = rospy.ServiceProxy('foot_aline_pose', C23_orient) # clone_service
self._BDIswitch_client = rospy.ServiceProxy('C25/BDIswitch', C25BDI)
state = Int32()
state.data = 1
resp_switched_to_BDI_odom = self._BDIswitch_client(state)
print "Using BDI odom"
# Subscribers:
#self._Subscribers["Odometry"] = rospy.Subscriber('/ground_truth_odom',Odometry,self._odom_cb)
self._Subscribers["ASI_State"] = rospy.Subscriber(
'/atlas/atlas_sim_interface_state', AtlasSimInterfaceState,
self.asi_state_cb)
self._Subscribers["IMU"] = rospy.Subscriber('/atlas/imu', Imu,
self._get_imu)
self._Subscribers["JointStates"] = rospy.Subscriber(
'/atlas/joint_states', JointState, self._get_joints)
rospy.sleep(0.3)
self._RequestTargetPose(self._DesiredObject)
self._k_effort = [0] * 28
self._k_effort[3] = 255
# self._k_effort[0:4] = 4*[255]
# self._k_effort[16:28] = 12*[255]
self._JC.set_k_eff(self._k_effort)
self._JC.set_all_pos(self._cur_jnt)
self._JC.send_command()
self._bDone = False
self._bIsSwaying = False
#self._bRobotIsStatic = False
#self._GetOrientationDelta0Values() # Orientation difference between BDI odom and Global
# Put robot into stand position
stand = AtlasSimInterfaceCommand(None, AtlasSimInterfaceCommand.STAND,
None, None, None, None,
self._k_effort)
self.asi_command.publish(stand)
rospy.sleep(0.3)
def __init__(self):
rospy.init_node('position_nav_node', anonymous=True)
rospy.on_shutdown(self.shutdown)
# How long in seconds should the robot pause at each location?
self.rest_time = rospy.get_param("~rest_time", 3)
# Are we running in the fake simulator?
self.fake_test = rospy.get_param("~fake_test", False)
# Goal state return values
goal_states = ['PENDING', 'ACTIVE', 'PREEMPTED',
'SUCCEEDED', 'ABORTED', 'REJECTED',
'PREEMPTING', 'RECALLING', 'RECALLED',
'LOST']
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
#
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
locations = dict()
locations['one-1'] = Pose(Point(-0.6353,-0.1005,0.0), Quaternion(0.0,0.0,0.9793,0.20249))
locations['one'] = Pose(Point(-1.4373,0.2436,0.0), Quaternion(0.0,0.0,0.9764,0.2159))
locations['two-1'] = Pose(Point(-0.6353,-0.1005,0.0), Quaternion(0.0,0.0,0.9793,0.20249))
locations['two'] = Pose(Point(-0.3821,-0.5335,0.0), Quaternion(0.0,0.0,-0.8500,0.5267))
locations['three-1'] = Pose(Point(-0.1248,0.4022,0.0), Quaternion(0.0,0.0,0.7374,0.67542))
locations['three'] = Pose(Point(-0.8292,1.0313,0.0), Quaternion(0.0,0.0,0.9744,0.2243))
locations['four-1'] = Pose(Point(-0.1248,0.4022,0.0), Quaternion(0.0,0.0,0.7374,0.67542))
locations['four-2'] = Pose(Point(0.5078,0.1495,0.0), Quaternion(0.0,0.0,0.9818,0.1898))
locations['four'] = Pose(Point(0.4435,0.3268,0.0), Quaternion(0.0,0.0,0.5583,0.8296))
locations['initial'] = Pose(Point(0.5078,0.1495,0.0), Quaternion(0.0,0.0,0.9818,0.1898))
# 2018.8.6 backhome code
# locations['back'] = initial_pose
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=10)
self.IOTnet_pub = rospy.Publisher('/IOT_cmd', IOTnet, queue_size=10)
self.initial_pub = rospy.Publisher('initialpose', PoseWithCovarianceStamped, queue_size=10)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
# A variable to hold the initial pose of the robot to be set by
# the user in RViz
initial_pose = PoseWithCovarianceStamped()
# Variables to keep track of success rate, running time,
# and distance traveled
n_locations = len(locations)
n_goals = 0
n_successes = 0
i = 0
distance_traveled = 0
start_time = rospy.Time.now()
running_time = 0
location = ""
last_location = ""
sequeue=['one-1','one','two-1','two','three-1','three', 'four-1', 'four-2' ,'four']
# Get the initial pose from the user
rospy.loginfo("*** Click the 2D Pose Estimate button in RViz to set the robot's initial pose...")
#rospy.wait_for_message('initialpose', PoseWithCovarianceStamped)
self.last_location = Pose()
rospy.Subscriber('initialpose', PoseWithCovarianceStamped, self.update_initial_pose)
setpose = PoseWithCovarianceStamped(Header(0,rospy.Time(),"map"), PoseWithCovariance(locations['initial'], [0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.25, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.06853891945200942]))
self.initial_pub.publish(setpose)
# Make sure we have the initial pose
rospy.sleep(1)
while initial_pose.header.stamp == "":
rospy.sleep(1)
rospy.sleep(1)
# locations['back'] = Pose()
rospy.loginfo("Starting position navigation ")
# Begin the main loop and run through a sequence of locations
while not rospy.is_shutdown():
# If we've gone through the all sequence, then exit
if i == n_locations:
rospy.logwarn("Now reach all destination, now exit...")
rospy.signal_shutdown('Quit')
break
# Get the next location in the current sequence
location = sequeue[i]
# Keep track of the distance traveled.
# Use updated initial pose if available.
if initial_pose.header.stamp == "":
distance = sqrt(pow(locations[location].position.x -
locations[last_location].position.x, 2) +
pow(locations[location].position.y -
locations[last_location].position.y, 2))
else:
rospy.loginfo("Updating current pose.")
distance = sqrt(pow(locations[location].position.x -
initial_pose.pose.pose.position.x, 2) +
pow(locations[location].position.y -
initial_pose.pose.pose.position.y, 2))
initial_pose.header.stamp = ""
# Store the last location for distance calculations
last_location = location
# Increment the counters
i += 1
n_goals += 1
# Set up the next goal location
self.goal = MoveBaseGoal()
self.goal.target_pose.pose = locations[location]
self.goal.target_pose.header.frame_id = 'map'
self.goal.target_pose.header.stamp = rospy.Time.now()
# Let the user know where the robot is going next
rospy.loginfo("Going to: " + str(location))
# Start the robot toward the next location
self.move_base.send_goal(self.goal) #move_base.send_goal()
# Allow 5 minutes to get there
finished_within_time = self.move_base.wait_for_result(rospy.Duration(300))
# Map to 4 point nav
cur_position = -1
position_seq = ['one','two','three','four']
if str(location) in position_seq:
cur_position = position_seq.index(str(location))+1
# Check for success or failure
if not finished_within_time:
self.move_base.cancel_goal() #move_base.cancle_goal()
rospy.loginfo("Timed out achieving goal")
else:
state = self.move_base.get_state() #move_base.get_state()
if state == GoalStatus.SUCCEEDED:
rospy.loginfo("Goal succeeded!")
n_successes += 1
distance_traveled += distance
rospy.loginfo("State:" + str(state))
if cur_position!=-1:
if cur_position == 3:
rospy.sleep(5)
self.IOTnet_pub.publish(cur_position)
rospy.sleep(12)
#if cur_position == 2:
# rospy.sleep(5)
else:
rospy.loginfo("Goal failed with error code: " + str(goal_states[state]))
if cur_position != -1:
if cur_position == 3:
rospy.sleep(5)
self.IOTnet_pub.publish(cur_position)
rospy.sleep(12)
# How long have we been running?
running_time = rospy.Time.now() - start_time
running_time = running_time.secs / 60.0
# Print a summary success/failure, distance traveled and time elapsed
rospy.loginfo("Success so far: " + str(n_successes) + "/" +
str(n_goals) + " = " +
str(100 * n_successes/n_goals) + "%")
rospy.loginfo("Running time: " + str(trunc(running_time, 1)) +
" min Distance: " + str(trunc(distance_traveled, 1)) + " m")
rospy.sleep(self.rest_time)
def run_ros():
rospy.init_node('ros_demo')
# First param: topic name; second param: type of the message to be published; third param: size of queued messages,
# at least 1
chatter_pub = rospy.Publisher('some_chatter', String, queue_size=10)
marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10)
path_points_pub = rospy.Publisher('path_points', PoseArray, queue_size=10)
# Each second, ros "spins" and draws 20 frames
loop_rate = rospy.Rate(20) # 20hz
frame_count = 0
obstacles = create_obstacles()
obstacles_lines = get_obstacles_lines(obstacles)
robot = create_robot()
target = create_target()
target_lines = get_target_lines(target)
point = get_point_structure()
tree_edges = get_tree_edges_structure()
p0 = Point(robot.pose.position.x, robot.pose.position.y, 0)
tree = Tree(TreeNode(p0))
collision_edges = get_collision_edges_structure()
found_path = False
path_edges = get_path_edges_structure()
drawed_path = False
robot_reached = False
path_published = False
path_points = []
while not rospy.is_shutdown():
msg = "Frame index: %s" % frame_count
rospy.loginfo(msg)
chatter_pub.publish(msg)
for obst in obstacles:
obst.header.stamp = rospy.Time.now()
marker_pub.publish(obst)
robot.header.stamp = rospy.Time.now()
target.header.stamp = rospy.Time.now()
marker_pub.publish(target)
point.header.stamp = rospy.Time.now()
tree_edges.header.stamp = rospy.Time.now()
collision_edges.header.stamp = rospy.Time.now()
path_edges.header.stamp = rospy.Time.now()
if frame_count % HZ == 0 and not found_path:
rand_pnt = Point()
rand_pnt.x = random.uniform(BOARD_CORNERS[0], BOARD_CORNERS[1])
rand_pnt.y = random.uniform(BOARD_CORNERS[3], BOARD_CORNERS[2])
rand_pnt.z = 0
point.points = [rand_pnt]
close_node = tree.get_closest_node(rand_pnt)
close_pnt = close_node.point
# https://math.stackexchange.com/questions/175896/finding-a-point-along-a-line-a-certain-distance-away-from-another-point
total_dist = math.sqrt(
math.pow(rand_pnt.x - close_pnt.x, 2) +
math.pow(rand_pnt.y - close_pnt.y, 2))
dist_ratio = STEP / total_dist
new_pnt = Point()
new_pnt.x = (1 -
dist_ratio) * close_pnt.x + dist_ratio * rand_pnt.x
new_pnt.y = (1 -
dist_ratio) * close_pnt.y + dist_ratio * rand_pnt.y
new_pnt.z = 0
if collides_object(close_pnt, new_pnt, obstacles_lines):
collision_edges.points.append(close_pnt)
collision_edges.points.append(new_pnt)
else:
last_node = tree.add_node(close_node, new_pnt)
tree_edges.points.append(close_pnt)
tree_edges.points.append(new_pnt)
if collides_object(close_pnt, new_pnt, target_lines):
found_path = True
if found_path and not drawed_path:
current_node = last_node
while not current_node.is_root():
path_points.append(current_node.point)
path_edges.points.append(current_node.point)
path_edges.points.append(current_node.parent.point)
current_node = current_node.parent
drawed_path = True
if found_path and drawed_path and not path_published:
path_poses = []
for i in range(len(path_points) - 1):
current_point = path_points[i]
next_point = path_points[i + 1]
current_pose = Pose()
current_pose.position = current_point
current_quat = Quaternion()
current_quat.x = 0
current_quat.y = 0
current_quat.z = 1
prev_angle = 0
if i > 0:
prev_angle = path_poses[i - 1].orientation.w
current_quat.w = np.arctan(
(next_point.y - current_point.y) /
(next_point.x - current_point.x)) - prev_angle
current_pose.orientation = current_quat
path_poses.append(current_pose)
path_pose_array = PoseArray()
path_pose_array.poses = path_poses
path_points_pub.publish(path_pose_array)
path_published = True
if frame_count % 2 == 0 and drawed_path and not robot_reached:
robot.pose.position = path_points.pop()
robot_reached = True if len(path_points) == 0 else False
if robot_reached:
break
marker_pub.publish(robot)
marker_pub.publish(point)
marker_pub.publish(tree_edges)
marker_pub.publish(collision_edges)
marker_pub.publish(path_edges)
# TODO: Robot function
"""
# From here, we are defining and drawing a simple robot
rob.type = rob.SPHERE
path.type = path.LINE_STRIP
rob.header.frame_id = "map"
path.header.frame_id = "map"
rob.header.stamp = rospy.Time.now()
path.header.stamp = rospy.Time.now()
rob.ns = "rob"
path.ns = "rob"
rob.id = 0
path.id = 1
rob.action = rob.ADD
path.action = path.ADD
rob.lifetime = rospy.Duration()
path.lifetime = rospy.Duration()
rob.scale.x, rob.scale.y, rob.scale.z = 0.3, 0.3, 0.3
rob.color.r, rob.color.g, rob.color.b, rob.color.a = 1.0, 0.5, 0.5, 1.0
# Path line strip is blue
path.color.b, path.color.a = 1.0, 1.0
path.scale.x = 0.02
path.pose.orientation.w = 1.0
num_slice2 = 200 # Divide a circle into segments
if frame_count % 2 == 0 and len(path.points) <= num_slice2:
p = Point()
angle = slice_index2 * 2 * math.pi / num_slice2
slice_index2 += 1
p.x = 4 * math.cos(angle) - 0.5 # Some random circular trajectory, with radius 4, and offset (-0.5, 1, .05)
p.y = 4 * math.sin(angle) + 1.0
p.z = 0.05
rob.pose.position = p
path.points.append(p) # For drawing path, which is line strip type
marker_pub.publish(rob)
marker_pub.publish(path)
"""
# To here, we finished displaying our components
# Check if there is a subscriber. Here our subscriber will be Rviz
while marker_pub.get_num_connections() < 1:
if rospy.is_shutdown():
return 0
# rospy.logwarn_once("Please run Rviz in another terminal.")
rospy.sleep(1)
loop_rate.sleep()
frame_count += 1
def pr2_mover(object_list):
# TODO: Initialize variables
dict_list = []
centroids = [] # to be list of tuples (x, y, z)
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
dict_dropbox = {}
for p in dropbox_param:
dict_dropbox[p['name']] = p['position']
#print (dict_dropbox["left"])
#print (dict_dropbox["right"])
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
for obj in object_list_param:
# TODO: Get the PointCloud for a given object and obtain it's centroid
object_name = String()
object_name.data = obj['name']
#print(object_list[object_name])
#set default value of pick_pose in case the object can't be found
pick_pose = Pose()
pick_pose.position.x = 0
pick_pose.position.y = 0
pick_pose.position.z = 0
#set orientation to 0
pick_pose.orientation.x = 0
pick_pose.orientation.y = 0
pick_pose.orientation.z = 0
pick_pose.orientation.w = 0
#set place pose orientation to 0
place_pose = Pose()
place_pose.orientation.x = 0
place_pose.orientation.y = 0
place_pose.orientation.z = 0
place_pose.orientation.w = 0
#print(object_name)
for detected_object in object_list:
if detected_object.label == object_name.data:
#print(object_name)
# TODO: Create 'place_pose' for the object
#labels.append(object.label)
points_arr = ros_to_pcl(detected_object.cloud).to_array()
#centroids.append(np.mean(points_arr, axis=0)[:3])
pick_pose_np = np.mean(points_arr, axis=0)[:3]
#pick_pose.position = [np.asscalar(pick_pose_np[0]), np.asscalar(pick_pose_np[1]), np.asscalar(pick_pose_np[2])]
pick_pose.position.x = np.asscalar(pick_pose_np[0])
pick_pose.position.y = np.asscalar(pick_pose_np[1])
pick_pose.position.z = np.asscalar(pick_pose_np[2])
#print(pick_pose.position)
break
# TODO: Assign the arm to be used for pick_place
arm_name = String()
if obj['group'] == 'red':
arm_name.data = 'left'
elif obj['group'] == 'green':
arm_name.data = 'right'
else:
print "ERROR, group must be green or red!"
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
test_scene_num = Int32()
test_scene_num.data = 3
place_pose.position.x = dict_dropbox[arm_name.data][0]
place_pose.position.y = dict_dropbox[arm_name.data][1]
place_pose.position.z = dict_dropbox[arm_name.data][2]
dict_list.append(make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose))
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
# try:
# pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
# # TODO: Insert your message variables to be sent as a service request
# resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE)
# print ("Response: ",resp.success)
# except rospy.ServiceException, e:
# print "Service call failed: %s"%e
# TODO: Output your request parameters into output yaml file
yaml_filename = "output_" + str(test_scene_num.data) + ".yaml"
send_to_yaml(yaml_filename, dict_list)
def __init__(self):
"""
Initialize the SCARA environemnt
NOTE: This environment uses ROS and interfaces.
TODO: port everything to ROS 2 natively
"""
# Launch the simulation with the given launchfile name
gazebo_env.GazeboEnv.__init__(self, "MARANoGripper_v0.launch")
# TODO: cleanup this variables, remove the ones that aren't used
# class variables
self._observation_msg = None
self.scale = None # must be set from elsewhere based on observations
self.bias = None
self.x_idx = None
self.obs = None
self.reward = None
self.done = None
self.reward_dist = None
self.reward_ctrl = None
self.action_space = None
self.max_episode_steps = 1000 # now used in all algorithms
self.iterator = 0
# default to seconds
self.slowness = 1
self.slowness_unit = 'sec'
self.reset_jnts = True
self._time_lock = threading.RLock()
#############################
# Environment hyperparams
#############################
# target, where should the agent reach
EE_POS_TGT = np.asmatrix([-0.4, 0.0, 1.1013]) # 200 cm from the z axis
# EE_POS_TGT = np.asmatrix([0.3305805, -0.1326121, 0.4868]) # center of the H
EE_ROT_TGT = np.asmatrix([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
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])
# Used to initialize the robot, #TODO, clarify this more
# STEP_COUNT = 2 # Typically 100.
# slowness = 10000000 # 10 ms, where 1 second is real life simulation
# slowness = 1000000 # 1 ms, where 1 second is real life simulation
# slowness = 1 # use >10 for running trained network in the simulation
# slowness = 10 # use >10 for running trained network in the simulation
# 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'
# Set constants for links
BASE = 'base_link'
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'
# EE_LINK = 'ee_link'
JOINT_ORDER = [MOTOR1_JOINT, MOTOR2_JOINT, MOTOR3_JOINT,
MOTOR4_JOINT, MOTOR5_JOINT, MOTOR6_JOINT]
LINK_NAMES = [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': []
}
#############################
# TODO: fix this and make it relative
# Set the path of the corresponding URDF file from "assets"
URDF_PATH = rospkg.RosPack().get_path("mara_description") + "/urdf/mara_robot_nogripper.urdf"
m_joint_order = copy.deepcopy(JOINT_ORDER)
m_link_names = copy.deepcopy(LINK_NAMES)
m_joint_publishers = copy.deepcopy(JOINT_PUBLISHER)
m_joint_subscribers = copy.deepcopy(JOINT_SUBSCRIBER)
ee_pos_tgt = EE_POS_TGT
ee_rot_tgt = EE_ROT_TGT
# Initialize target end effector position
ee_tgt = np.ndarray.flatten(get_ee_points(EE_POINTS, ee_pos_tgt, ee_rot_tgt).T)
self.realgoal = ee_tgt
self.environment = {
# rk changed this to for the mlsh
# 'ee_points_tgt': ee_tgt,
'ee_points_tgt': self.realgoal,
'joint_order': m_joint_order,
'link_names': m_link_names,
# 'slowness': slowness,
'reset_conditions': reset_condition,
'tree_path': URDF_PATH,
'joint_publisher': m_joint_publishers,
'joint_subscriber': m_joint_subscribers,
'end_effector_points': EE_POINTS,
'end_effector_velocities': EE_VELOCITIES,
}
# self.spec = {'timestep_limit': 5, 'reward_threshold': 950.0,}
# Subscribe to the appropriate topics, taking into account the particular robot
# ROS 1 implementation
self._pub = rospy.Publisher(JOINT_PUBLISHER, JointTrajectory)
self._sub = rospy.Subscriber(JOINT_SUBSCRIBER, JointTrajectoryControllerState, self.observation_callback)
# 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 = treeFromFile(self.environment['tree_path'])
# Retrieve a chain structure between the base and the start of the end effector.
self.scara_chain = self.ur_tree.getChain(self.environment['link_names'][0], self.environment['link_names'][-1])
print("nr of jnts: ", self.scara_chain.getNrOfJoints())
# Initialize a KDL Jacobian solver from the chain.
self.jac_solver = ChainJntToJacSolver(self.scara_chain)
#print(self.jac_solver)
self._observations_stale = [False for _ in range(1)]
#print("after observations stale")
self._currently_resetting = [False for _ in range(1)]
self.reset_joint_angles = [None for _ in range(1)]
# TODO review with Risto, we might need the first observation for calling step()
# observation = self.take_observation()
# assert not done
# self.obs_dim = observation.size
"""
obs_dim is defined as:
num_dof + end_effector_points=3 + end_effector_velocities=3
end_effector_points and end_effector_velocities is constant and equals 3
"""
#
self.obs_dim = self.scara_chain.getNrOfJoints() + 6 # hardcode it for now
# # print(observation, _reward)
# # 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.]
# #bounds = self.model.actuator_ctrlrange.copy()
low = -np.pi/2.0 * np.ones(self.scara_chain.getNrOfJoints())
high = np.pi/2.0 * np.ones(self.scara_chain.getNrOfJoints())
# print("Action spaces: ", low, high)
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.add_model = rospy.ServiceProxy('/gazebo/spawn_urdf_model', SpawnModel)
self.remove_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel)
model_xml = "<?xml version=\"1.0\"?> \
<robot name=\"myfirst\"> \
<link name=\"world\"> \
</link>\
<link name=\"cylinder0\">\
<visual>\
<geometry>\
<sphere radius=\"0.01\"/>\
</geometry>\
<origin xyz=\"0 0 0\"/>\
<material name=\"rojotransparente\">\
<ambient>0.5 0.5 1.0 0.1</ambient>\
<diffuse>0.5 0.5 1.0 0.1</diffuse>\
</material>\
</visual>\
<inertial>\
<mass value=\"5.0\"/>\
<inertia ixx=\"1.0\" ixy=\"0.0\" ixz=\"0.0\" iyy=\"1.0\" iyz=\"0.0\" izz=\"1.0\"/>\
</inertial>\
</link>\
<joint name=\"world_to_base\" type=\"fixed\"> \
<origin xyz=\"0 0 0\" rpy=\"0 0 0\"/>\
<parent link=\"world\"/>\
<child link=\"cylinder0\"/>\
</joint>\
<gazebo reference=\"cylinder0\">\
<material>Gazebo/GreenTransparent</material>\
</gazebo>\
</robot>"
robot_namespace = ""
pose = Pose()
pose.position.x = EE_POS_TGT[0,0];
pose.position.y = EE_POS_TGT[0,1];
pose.position.z = EE_POS_TGT[0,2];
#Static obstacle (not in original code)
# pose.position.x = 0.25;#
# pose.position.y = 0.07;#
# pose.position.z = 0.0;#
pose.orientation.x = 0;
pose.orientation.y= 0;
pose.orientation.z = 0;
pose.orientation.w = 0;
reference_frame = ""
rospy.wait_for_service('/gazebo/spawn_urdf_model')
self.add_model(model_name="target",
model_xml=model_xml,
robot_namespace="",
initial_pose=pose,
reference_frame="")
# Seed the environment
# Seed the environment
self._seed()
def __init__(self):
# Name this node, it must be unique
rospy.init_node('autonomous', anonymous=True)
# Enable shutdown in rospy (This is important so we cancel any move_base goals
# when the node is killed)
rospy.on_shutdown(self.shutdown)
self.rest_time = rospy.get_param("~rest_time",
0.1) # Minimum pause at each location
self.stalled_threshold = rospy.get_param("~stalled_threshold",
100) # Loops before stall
# Goal state return values
goal_states = [
'PENDING', 'ACTIVE', 'PREEMPTED', 'SUCCEEDED', 'ABORTED',
'REJECTED', 'PREEMPTING', 'RECALLING', 'RECALLED', 'LOST'
]
# Set up the goal locations. Poses are defined in the map frame.
# An easy way to find the pose coordinates is to point-and-click
# Nav Goals in RViz when running in the simulator.
# Pose coordinates are then displayed in the terminal
# that was used to launch RViz.
self.waypoints = list()
self.tf = TransformListener()
# self.locations = dict()
# self.wpname = dict()
rospack = rospkg.RosPack()
f = open(
rospack.get_path('husky_wp') + '/params/pre-defined-standby.txt',
'r')
# ct2 = 0
with f as openfileobject:
first_line = f.readline()
for line in openfileobject:
nome = [x.strip() for x in line.split(',')]
#self.wpname[ct2] = nome[0]
x = Decimal(nome[1])
y = Decimal(nome[2])
z = Decimal(nome[3])
X = Decimal(nome[4])
Y = Decimal(nome[5])
Z = Decimal(nome[6])
#self.locations[self.wpname[ct2]] = Pose(Point(x,y,z), Quaternion(X,Y,Z,1))
self.waypoints.append(
Pose(Point(x, y, z), Quaternion(0, 0, 0, 1)))
#print self.waypoints
#time.sleep(1)
# ct2 = ct2+1
#self.wp = -1
#self.ct4 = 0
print "Static path has : "
print len(self.waypoints)
print " point(s)."
time.sleep(5)
# Publisher to manually control the robot (e.g. to stop it, queue_size=5)
self.cmd_vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=5)
# Subscribe to the move_base action server
self.move_base = actionlib.SimpleActionClient("move_base",
MoveBaseAction)
rospy.loginfo("Waiting for move_base action server...")
# Wait 60 seconds for the action server to become available
self.move_base.wait_for_server(rospy.Duration(60))
rospy.loginfo("Connected to move base server")
rospy.loginfo("Starting navigation test")
# Initialize a counter to track waypoints
i = 0
# Cycle through the four waypoints
while i < len(self.waypoints) and not rospy.is_shutdown():
# Update the marker display
#self.marker_pub.publish(self.markers)
time.sleep(2)
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'odom'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
# Set the goal pose to the i-th waypoint
goal.target_pose.pose = self.waypoints[i]
# Start the robot moving toward the goal
self.move(goal)
i += 1
# check if the goal point is found by the detect_goal node
if rospy.has_param('/panel_goal'):
# go to goal the goal goint
# get parameter
panel_goal = rospy.get_param("/panel_goal")
print panel_goal[0]
print panel_goal[1]
print panel_goal[2]
while not rospy.is_shutdown():
try:
# Find the global coordinate of the UGV
(trans, rot) = self.tf.lookupTransform(
'/odom', '/base_link', rospy.Time(0))
# Calculate the vector between the UGV and the goal point
Tx = panel_goal[0] - trans[0]
Ty = panel_goal[1] - trans[1]
print "Vehicle global coordinates is:"
print trans[0]
print trans[1]
print "The travel vecor is:"
print Tx
print Ty
# Calculate the travel distance between the UGV and the goal point
travel_distance = math.sqrt(
math.pow(Tx, 2) + math.pow(Ty, 2))
print "The travel distance is:"
print travel_distance
# Calculate a scaling vector to make the UGV stop at 1.5m away from the goal.
travel_distance2 = travel_distance - 1.5
factor = travel_distance2 / travel_distance
print "The scaliing factor is:"
print factor
Tx = Tx * factor
Ty = Ty * factor
# Calulate the goal point in the global frame
goal_x = trans[0] + Tx
goal_y = trans[1] + Ty
# Intialize the waypoint goal
goal = MoveBaseGoal()
# Use the map frame to define goal poses
goal.target_pose.header.frame_id = 'odom'
# Set the time stamp to "now"
goal.target_pose.header.stamp = rospy.Time.now()
print "The goal is:"
print goal_x
print goal_y
goal.target_pose.pose = Pose(Point(goal_x, goal_y, 0),
Quaternion(0, 0, 0, 1))
self.move(goal)
rospy.loginfo("Reach goal")
break
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.loginfo("No transformation")
break
else:
rospy.loginfo("No goal found")
def __init__(self, x, y, z):
self._x = x
self._y = y
self._z = z
# Retrieve params:
self._table_object_name = rospy.get_param('~table_object_name',
'Grasp_Table')
self._grasp_object_name = rospy.get_param('~grasp_object_name',
'Grasp_Object')
self._grasp_object_width = rospy.get_param('~grasp_object_width', 0.01)
self._arm_group = rospy.get_param('~manipulator', 'manipulator')
self._gripper_group = rospy.get_param('~gripper', 'gripper')
self._approach_retreat_desired_dist = rospy.get_param(
'~approach_retreat_desired_dist', 0.15)
self._approach_retreat_min_dist = rospy.get_param(
'~approach_retreat_min_dist', 0.1)
# Create (debugging) publishers:创建（调试）发布者：
self._grasps_pub = rospy.Publisher('grasps',
PoseArray,
queue_size=1,
latch=True)
self._places_pub = rospy.Publisher('places',
PoseArray,
queue_size=1,
latch=True)
# Create planning scene and robot commander:创建规划场景和机器人命令：
self._scene = PlanningSceneInterface() #未知
self._robot = RobotCommander() #未知
self._pose_coke_can = self._add_grasp_block_(self._grasp_object_name,
self._x, self._y, self._z)
rospy.sleep(0.5)
# Define target place pose:定义目标位置姿势
self._pose_place = Pose()
self._pose_place.position.x = -0.75 #self._pose_coke_can.position.x
self._pose_place.position.y = 0.0 #self._pose_coke_can.position.y - 0.20
self._pose_place.position.z = self._z #self._pose_coke_can.position.z
self._pose_place.orientation = Quaternion(
*quaternion_from_euler(0.0, 0.0, 0.0))
self._pose_coke_can.position.z = self._pose_coke_can.position.z - 0.9 #self._pose_coke_can.position.z - 0.9 # base_link is 0.9 above
self._pose_place.position.z = self._pose_place.position.z + 0.05 # target pose a little above
# Retrieve groups (arm and gripper):
self._arm = self._robot.get_group(self._arm_group)
self._gripper = self._robot.get_group(self._gripper_group)
self._arm.set_named_target('up')
self._arm.go(wait=True)
print("up pose")
print("第一部分恢复位置初始")
# Create grasp generator 'generate' action client:
# #创建抓取生成器“生成”动作客户端：
print("开始Action通信")
self._grasps_ac = SimpleActionClient(
'/moveit_simple_grasps_server/generate', GenerateGraspsAction)
if not self._grasps_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Grasp generator action client not available!')
rospy.signal_shutdown(
'Grasp generator action client not available!')
return
print("结束Action通信")
# Create move group 'pickup' action client:
# 创建移动组“抓取”操作客户端：
print("开始pickup通信")
self._pickup_ac = SimpleActionClient('/pickup', PickupAction)
if not self._pickup_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Pick up action client not available!')
rospy.signal_shutdown('Pick up action client not available!')
return
print("结束pickup通信")
# Create move group 'place' action client:
# 创建移动组“放置”动作客户端：
print("开始place通信")
self._place_ac = SimpleActionClient('/place', PlaceAction)
if not self._place_ac.wait_for_server(rospy.Duration(0.5)):
rospy.logerr('Place action client not available!')
rospy.signal_shutdown('Place action client not available!')
return
print("结束place通信")
# Pick Coke can object:抓取快
while not self._pickup(self._arm_group, self._grasp_object_name,
self._grasp_object_width):
rospy.logwarn('Pick up failed! Retrying ...')
rospy.sleep(0.5)
print("抓取物体")
rospy.loginfo('Pick up successfully')
print("pose_place: ", self._pose_place)
# Place Coke can object on another place on the support surface (table):
while not self._place(self._arm_group, self._grasp_object_name,
self._pose_place):
rospy.logwarn('Place failed! Retrying ...')
rospy.sleep(0.5)
rospy.loginfo('Place successfully')
for place in places:
msg.poses.append(place.place_pose.pose)
self._places_pub.publish(msg)
if __name__ == '__main__':
roscpp_initialize(sys.argv)
rospy.init_node('pick_and_place')
rospack = rospkg.RosPack()
pack_path = rospack.get_path('ur5_single_arm_tufts')
table_path = pack_path + os.sep + 'urdf' + os.sep + 'objects' + os.sep + 'table.urdf'
table_name = 'table'
table_pose = Pose(position=Point(
x=0.85, y=0.0,
z=0.70 + 0.03)) # increase z by 0.03 to make gripper reach block
fangkuai_path = pack_path + os.sep + 'urdf' + os.sep + 'objects' + os.sep + 'block.urdf'
fangkuai_name = 'fangkuai'
fangkuai_pose = Pose(position=Point(x=0.8, y=-0.4, z=0.74 + 0.03))
gold_block_path = pack_path + os.sep + 'urdf' + os.sep + 'objects' + os.sep + 'gold_block.urdf'
gold_block_name = 'gold_block'
gold_block_pose = Pose(position=Point(x=0.8, y=-0.2, z=0.74 + 0.03))
delete_gazebo_model([table_name, fangkuai_name, gold_block_name])
spawn_gazebo_model(table_path, table_name, table_pose)
spawn_gazebo_model(fangkuai_path, fangkuai_name, fangkuai_pose)
spawn_gazebo_model(gold_block_path, gold_block_name, gold_block_pose)
"""
rosrun gazebo_ros spawn_model -file $(rospack find ur5_single_arm_tufts)/urdf/objects/table.urdf -urdf -x 0.85 -y 0.0 -z 0.73 -model my_object
rosrun gazebo_ros spawn_model -file $(rospack find ur5_single_arm_tufts)/urdf/objects/block.urdf -urdf -x 0.5 -y -0.0 -z 0.77 -model block
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 + 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)
# 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 update_odom(self):
t2 = rospy.Time.now()
t1 = self.prev_time
dt = (t2 - t1).to_sec()
gyro_thresh = 0.05
g_bias = 0.007
MAX_DTHETA_GYRO = 5
try:
self.ardIMU.safe_write('A5/6/')
gyroz = self.ardIMU.safe_read()
#c = 0
#print("gyro z:")
#print(gyroz)
#while(gyroz == '' and c < 10):
# c = c+1
# gyroz = ardIMU.safe_read()
# print('try gyro again')
gyroz = float(int(gyroz) - 1000) / 100.0
except:
gyroz = 0
print 'IMU error'
print "Unexpected Error:", sys.exc_info()[0]
finally:
a = 0
# Read encoder delta
try:
self.ard.safe_write('A3/4/')
s = self.ard.safe_read()
#print("enc: ")
#print(s)
c = 0
#while(s == '' and c < 10):
# c = c +1
# s = ardIMU.safe_read()
# print('try enc again')
delta_enc_counts = int(s)
except:
delta_enc_counts = 0
print 'enc error'
print "Unexpected Error:", sys.exc_info()[0]
finally:
a = 0
# Update odom
self.enc_total = self.enc_total + delta_enc_counts
dmeters = float(delta_enc_counts) / 53.0 #53 counts/meter
if (abs(gyroz) < gyro_thresh):
gz_dps = 0
dtheta_gyro_deg = 0
else:
gz_dps = (gyroz + g_bias) * 180 / 3.14
dtheta_gyro_deg = gz_dps * dt
if (abs(dtheta_gyro_deg) > MAX_DTHETA_GYRO):
#print 'no gyro'
dtheta_deg = 0
use_gyro_flag = False
else:
#print 'use gyro'
dtheta_deg = dtheta_gyro_deg
use_gyro_flag = True
#update bot position
self.bot.move(dmeters, dtheta_deg, use_gyro_flag)
self.bot.servo_deg = 0
# update bot linear x velocity every 150 msec
# need to use an np array, then push and pop, moving average
self.dist_sum = self.dist_sum + dmeters
self.time_sum = self.time_sum + dt
if (self.time_sum > 0.15):
self.vx = self.dist_sum / self.time_sum
self.dist_sum = 0
self.time_sum = 0
#bot.botx*100,bot.boty*100,bot.bot_deg
odom_quat = tf.transformations.quaternion_from_euler(
0, 0, self.bot.bot_deg * 3.14 / 180.0)
self.odom_broadcaster.sendTransform((self.bot.botx, self.bot.boty, 0.),
odom_quat, t2, "base_link", "odom")
odom = Odometry()
odom.header.stamp = t2
odom.header.frame_id = "odom"
# set the position
odom.pose.pose = Pose(Point(self.bot.botx, self.bot.boty, 0.),
Quaternion(*odom_quat))
# set the velocity
odom.child_frame_id = "base_link"
odom.twist.twist = Twist(Vector3(self.vx, 0, 0),
Vector3(0, 0, gz_dps * 3.14 / 180.0))
# publish the message
self.odom_pub.publish(odom)
self.prev_time = t2
