encoder_msg_1 = Encoder()
encoder_msg_2 = Encoder()
encoder_msg_3 = Encoder()
motor_msg_1 = Cmd()
motor_msg_2 = Cmd()
motor_msg_3 = Cmd()
baro_pub = rospy.Publisher('barometer', Baro, queue_size=10)
baro_msg = Baro()
imu_pub = rospy.Publisher('imu', Imu, queue_size=10)
imu_msg = Imu()
pos_pub = rospy.Publisher('position', Point32, queue_size=10)
pos_msg = Point32()
rate = rospy.Rate(10)
while not rospy.is_shutdown():
t = rospy.get_time()
motor_msg_1.mode = 2
motor_msg_1.flipping_speed = 10 * np.cos(t)
motor_pub_1.publish(motor_msg_1)
encoder_msg_1.encoder_angle = 10 * np.sin(t)
encoder_msg_1.encoder_speed = 10 * np.cos(t + 0.1)
enc_pub_1.publish(encoder_msg_1)
motor_msg_2.mode = 2
motor_msg_2.flipping_speed = 20 * np.cos(t)
def point(npt):
pt = Point32()
pt.x = npt[0]
pt.y = npt[1]
return pt
print '[ INFO ] Publishing to {}'.format(topic_name)
self.odom_msg = Odometry()
def odom_cb(self, msg):
self.odom_msg = msg
if __name__ == '__main__':
rospy.init_node('global_pos_node', anonymous=True)
global_position = Global_Position()
rate = rospy.Rate(100)
while not rospy.is_shutdown():
loc_msg = Point32()
loc_msg.x = global_position.odom_msg.pose.pose.position.x
loc_msg.y = global_position.odom_msg.pose.pose.position.y
loc_msg.z = 0
stamped_loc_msg = PointStamped()
stamped_loc_msg.header.frame_id = str(global_position.agent_index)
stamped_loc_msg.point.x = global_position.odom_msg.pose.pose.position.x
stamped_loc_msg.point.y = global_position.odom_msg.pose.pose.position.y
stamped_loc_msg.point.z = 0
global_position.loc_pub.publish(loc_msg)
global_position.stamped_loc_pub.publish(stamped_loc_msg)
global_position.n_agts_pub.publish(3)
def listener_target():
point = Point32()
rospy.Subscriber('/seg_processing', Point32, callback)
def kinematicCalculation(self, objMsg, js, cortexMsg, rconfig=None):
self.points2D = [[p.x, p.y] for p in objMsg.pos2D]
pitch = math.radians(cortexMsg.pitch) if cortexMsg is not None else 0.0
roll = math.radians(cortexMsg.roll) if cortexMsg is not None else 0.0
# print math.degrees(pitch), math.degrees(roll)
# roll = pitch = 0.0
tranvec = np.zeros((3, 1))
rotvec = np.array([roll, pitch, 0.0])
HRotate = self.create_transformationMatrix(tranvec,
rotvec,
'rpy',
order="tran-first")
H = getMatrixForForwardKinematic(js.position[0], js.position[1], roll,
pitch)
H = np.matmul(HRotate, H)
points3D = self.calculate3DCoor(self.points2D, HCamera=H)
polarList = []
cartList = []
names = []
confidences = []
errorList = []
pos2DList = []
imgH = objMsg.imgH
imgW = objMsg.imgW
for (plane,
p3D), name, confidence, p2D in zip(points3D, objMsg.object_name,
objMsg.object_confidence,
objMsg.pos2D):
if plane is None:
pass
else:
x, y = p3D[:2]
if math.sqrt(x**2 + y**2) > 6:
continue
cartList.append(Point32(x=x, y=y))
rho = math.sqrt(x**2 + y**2)
phi = math.atan2(y, x)
polarList.append(Point32(x=rho, y=phi))
names.append(name)
confidences.append(confidence)
errorList.append(Point32(x=p2D.x / imgW, y=p2D.y / imgH))
pos2DList.append(p2D)
# print points3D
msg = postDictMsg()
msg.object_name = names
msg.pos3D_cart = cartList
msg.pos2D_polar = polarList
msg.pos2D = pos2DList
msg.object_error = objMsg.object_error
msg.object_confidence = confidences
msg.imgH = imgH
msg.imgW = imgW
return msg
def run(self):
rate = rospy.Rate(20) # 10hz
prev_diff = 0.0
done_turn_flag = False
while not rospy.is_shutdown():
if self.left_wall_scan.ranges and self.right_wall_scan.ranges:
left_wall_ranges_array = np.asarray(self.left_wall_scan.ranges)
right_wall_ranges_array = np.asarray(
self.right_wall_scan.ranges)
## remove zeros out from array
left_wall_ranges_array = left_wall_ranges_array[
left_wall_ranges_array != 0]
right_wall_ranges_array = right_wall_ranges_array[
right_wall_ranges_array != 0]
left_wall_ranges_min = np.min(left_wall_ranges_array)
right_wall_ranges_min = np.min(right_wall_ranges_array)
if self.ROBOT_MODE == "FRONT":
if self.FRONT_STOP:
self.sbus_cmd.data = [
self.sbus_steering_mid, self.sbus_throttle_mid
]
stop_timeout = time.time() - tic
print("Stop with time {:.3f}".format(stop_timeout))
if stop_timeout > 3.0:
self.ROBOT_MODE = "UTURN"
start_ang = self.hdg # capture starting angle before turning
done_turn_flag = False # set turning flag to False
self.pid.auto_mode = False # turn off pid
else:
tic = time.time()
diff = left_wall_ranges_min - right_wall_ranges_min
output_pid = self.pid(diff)
sbus_steering = int(
self.map_with_limit(output_pid, -1.0, 1.0,
self.sbus_steering_max,
self.sbus_steering_min))
sbus_throttle = self.sbus_throttle_fwd_const
self.sbus_cmd.data = [sbus_steering, sbus_throttle]
print("min_left: {:.3f} | min_right: {:.3f} | diff: {:.3f} | output_pid: {:.2f} | str: {:d} | thr: {:d}".format(\
left_wall_ranges_min, right_wall_ranges_min, diff, output_pid, sbus_steering, sbus_throttle))
if left_wall_ranges_min > 1.0 or right_wall_ranges_min > 1.0:
self.ROBOT_MODE = "LEFT_FOLLOW"
self.pid.auto_mode = False # disable distance keeping PID
self.pid_wf.auto_mode = True # enable wall following PID
self.sbus_cmd.data = [
self.sbus_steering_mid, self.sbus_throttle_mid
]
elif self.ROBOT_MODE == "UTURN":
if not done_turn_flag:
print("hdg: {:.3f} | start_ang: {:.3f} ".format(
self.hdg, start_ang))
self.sbus_cmd.data, done_turn_flag = self.turnLeft180Deg(
start_ang, self.hdg)
if done_turn_flag:
self.pid.auto_mode = True
self.ROBOT_MODE = "FRONT"
elif self.ROBOT_MODE == "LEFT_FOLLOW":
print("start wall follow mode")
output_pid_wf = self.pid_wf(left_wall_ranges_min)
sbus_steering = int(
self.map_with_limit(output_pid_wf, -1.0, 1.0,
self.sbus_steering_max,
self.sbus_steering_min))
sbus_throttle = self.sbus_throttle_fwd_const
print("min_left: {:.3f} | output_pid_wf: {:.2f} | str: {:d} | thr: {:d}".format(\
left_wall_ranges_min, output_pid_wf, sbus_steering, sbus_throttle))
self.sbus_cmd.data = [sbus_steering, sbus_throttle]
# wf_tic = time.time()
if left_wall_ranges_min < 1.0 and right_wall_ranges_min < 1.0:
# wf_timeout = time.time() - wf_tic
# if wf_timeout > 1.0:
self.ROBOT_MODE = "FRONT"
self.pid_wf.auto_mode = False
self.pid.auto_mode = True
else:
self.sbus_cmd.data = [
self.sbus_steering_mid, self.sbus_throttle_mid
]
# prev_diff = diff
self.left_wall_scan_pub.publish(self.left_wall_scan)
self.right_wall_scan_pub.publish(self.right_wall_scan)
self.sbus_cmd_pub.publish(self.sbus_cmd)
## Drawing Polygons ##
# footprint
self.pg.header.stamp = rospy.Time.now()
self.pg.header.frame_id = "base_footprint"
self.pg.polygon.points = [
Point32(x=A[0], y=A[1]),
Point32(x=B[0], y=B[1]),
Point32(x=D[0], y=D[1]),
Point32(x=C[0], y=C[1])
]
# front stop zone
self.pg_front_stop.header.stamp = rospy.Time.now()
self.pg_front_stop.header.frame_id = "base_footprint"
self.pg_front_stop.polygon.points = [
Point32(x=B[0], y=B[1]),
Point32(x=T[0], y=T[1]),
Point32(x=D[0], y=D[1]),
Point32(x=B[0], y=B[1])
]
# back stop zone
self.pg_back_stop.header.stamp = rospy.Time.now()
self.pg_back_stop.header.frame_id = "base_footprint"
self.pg_back_stop.polygon.points = [
Point32(x=A[0], y=A[1]),
Point32(x=C[0], y=C[1]),
Point32(x=W[0], y=W[1]),
Point32(x=A[0], y=A[1])
]
# construct tf
self.t.header.frame_id = "base_footprint"
self.t.header.stamp = rospy.Time.now()
self.t.child_frame_id = "base_link"
self.t.transform.translation.x = 0.0
self.t.transform.translation.y = 0.0
self.t.transform.translation.z = 0.0
self.t.transform.rotation.x = 0.0
self.t.transform.rotation.y = 0.0
self.t.transform.rotation.z = 0.0
self.t.transform.rotation.w = 1.0
self.br.sendTransform(self.t)
# laser_pub.publish(new_scan)
self.foot_pub.publish(self.pg)
self.front_stop_pub.publish(self.pg_front_stop)
self.back_stop_pub.publish(self.pg_back_stop)
rate.sleep()
def __init__(self):
# Adjustable Parameters
self.directory = rospy.get_param("~directory")
self.battery_threshold = float(
rospy.get_param("~battery_threshold", 30)) # [%]
self.timer = float(rospy.get_param("~timer", 10)) # [sec]
# Define Paths' File Name
self.path_dict = [
"HOME_A", "HOME_B", "HOME_1", "HOME_2", "HOME_3", "HOME_4",
"HOME_5", "A_1", "A_2", "A_3", "A_4", "A_5", "B_1", "B_2", "B_3",
"B_4", "B_5", "1_A", "2_A", "3_A", "4_A", "5_A", "1_B", "2_B",
"3_B", "4_B", "5_B", "A_HOME", "B_HOME", "1_HOME", "2_HOME",
"3_HOME", "4_HOME", "5_HOME"
]
# self.region_1 = Polygon(Point32(-2,2,0), Point32(10,2,0), Point32(10,-10,0), Point32(5,-10,0), Point32(5,-2,0), Point32(-2,-2,0))
# self.region_2 = Polygon(Point32(1,-2,0), Point32(1,-10,0), Point32(5,-10,0), Point32(5,-2,0))
# self.region_3 = Polygon(Point32(-2,-2,0), Point32(), Point32(), Point32())
# self.location_dict = {"store": self.region_1, "outdoor": self.region_2, "workshop": self.region_3}
# Define Actions Type
self.action_dict = {0: "None", 1: "Table_Picking", 2: "Table_Dropping"}
# Internal USE Variables - Modify with consultation
self.rate = rospy.Rate(5) # 5 [hz] <--> 0.2 [sec]
self.tf2Buffer = tf2_ros.Buffer()
self.tf2Listener = tf2_ros.TransformListener(self.tf2Buffer)
self.battery_level = "0.0"
self.battery_safe = True
self.table_took = 1
self.tablecall = 0
self.route_list = []
self.route_seq = 0
self.action_list = []
self.action_seq = 0
self.waypoint_list = []
self.waypoint_seq = 0
self.delivery_status = ""
self.action_status = True
self.last_msg = None
self.route_once = True
self.XYZ = Point32()
self.location = "HOME"
self.speed = 0
self.delivery_id = "0"
self.delivery_mission = ""
self.mission_activity = "NO ACTION"
# Publisher
self.all_pub = rospy.Publisher("/web/all_status", String, queue_size=1)
# Subscribers
self.battery_sub = rospy.Subscriber("/battery/percent",
String,
self.batteryCB,
queue_size=1)
self.mission_sub = rospy.Subscriber("/web/mission",
String,
self.missionCB,
queue_size=1)
self.fsm_sub = rospy.Subscriber("/fsm_node/state",
FSMState,
self.fsmCB,
queue_size=1)
self.odom_sub = rospy.Subscriber("/gyro/odom",
Odometry,
self.odomCB,
queue_size=1)
# Service Servers
self.route_done_srv = rospy.Service("/route/done", Empty,
self.route_doneSRV)
self.action_done_srv = rospy.Service("/action/done", Empty,
self.action_doneSRV)
self.charging_done_srv = rospy.Service("/charging/done", Empty,
self.charging_doneSRV)
self.pick_table_done_srv = rospy.Service("/pick_table/done", Empty,
self.action_doneSRV)
self.drop_table_done_srv = rospy.Service("/drop_table/done", Empty,
self.action_doneSRV)
# Service Clients
self.path_call = rospy.ServiceProxy("/change_path", ChangePath)
self.charging_call = rospy.ServiceProxy("charging/call", Empty)
self.pick_table_call = rospy.ServiceProxy("/pick_table/call", Empty)
self.drop_table_call = rospy.ServiceProxy("/drop_table/call", Empty)
self.rosbag_start_call = rospy.ServiceProxy("/rosbag/start", Empty)
self.rosbag_stop_call = rospy.ServiceProxy("/rosbag/stop", Empty)
self.fsm_call = rospy.ServiceProxy("/fsm_node/set_state", SetFSMState)
# Main Loop()
self.main_loop()
def getting_cordi(A, B, shu):
theta_increment = shu
re = PolygonArray()
re.header.frame_id = "base_scan"
roar = PolygonStamped()
roar.header.frame_id = "base_scan"
pt = Exp_msg()
count = 0
baby = Ipoly()
for i in range(len(A)):
if str(A[i]) != "inf":
if count == 0:
for t in range(3, 1, -1):
theta1 = (i - t) * theta_increment
x = A[i] * math.cos(theta1)
y = A[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
count += 1
theta1 = (i) * theta_increment
x = A[i] * math.cos(theta1)
y = A[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
if i == len(A) - 1 or str(
A[i + 1]) == "inf" or abs(A[i + 1] - A[i]) > 0.5:
for t in range(1, 5):
theta1 = (t + i) * theta_increment
x = A[i] * math.cos(theta1)
y = A[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
for t in range(3, 0, -1):
theta1 = (t + i) * theta_increment
x = B[i] * math.cos(theta1)
y = B[i] * math.sin(theta1)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
for j in range(i, i - count, -1):
theta2 = (j) * theta_increment
x = B[j] * math.cos(theta2)
y = B[j] * math.sin(theta2)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
for t in range(-1, -3, -1):
theta2 = (j + t) * theta_increment
x = B[j] * math.cos(theta2)
y = B[j] * math.sin(theta2)
roar.polygon.points.append(Point32(x, y, 0))
pt.bliss.append(Cordi(x, y, 0))
baby.eternal_bliss.append(pt)
pt = Exp_msg()
count = 0
re.polygons.append(roar)
roar = PolygonStamped()
roar.header.frame_id = "base_scan"
pubf = rospy.Publisher("ol2", Ipoly, queue_size=0)
pubf.publish(baby)
# print "bot yaw calculator ", bot_yaw
final_polys = PolygonArray()
# print "New"
for p in re.polygons:
# print " "
temp_poly = PolygonStamped()
temp_poly.header.frame_id = "odom"
for point in p.polygon.points:
temp_point = numpy.array([[point.x], [point.y]])
# print temp_point
# print point.x, point.y
# print "bot yaw calculator ", bot_yaw
cos = math.cos(-bot_yaw)
sin = math.sin(-bot_yaw)
# print cos, sin
rot_mat = numpy.array([[cos, sin], [-sin, cos]])
# rot_mat=numpy.array([[1 , 0],[0 , 1]])
trans_mat = numpy.array([[bot_x], [bot_y]])
rot_point = numpy.matmul(rot_mat, temp_point)
# print rot_point
trans_point = rot_point + trans_mat
# print trans_point[0], trans_point[1]
# point.x = trans_point[0]
# point.y = trans_point[1]
t_point = Point32()
t_point.x = trans_point[0]
t_point.y = trans_point[1]
temp_poly.polygon.points.append(t_point)
final_polys.polygons.append(temp_poly)
final_polys.header.frame_id = "odom"
return final_polys
def ImageProcessingFunction( self, img, header ):
startTime = time.time()
# get image property
imageWidth = img.shape[ 1 ]
imageHeight = img.shape[ 0 ]
# initial final position
bestPosition = Point32()
ballErrorList = Point32()
ballConfidence = 0.0
# blur image and change to hsv format
blurImage = cv2.GaussianBlur( img, ( 5, 5 ), 0 )
# segmentation and get marker
marker = colorSegmentation( blurImage, self.colorDefList )
# get field boundery, green color ID is 2
fieldContour, fieldMask = findBoundary( marker, 2, flip = False )
fieldContourMiddle = fieldContour[ 1:-1 ].copy()
#
# create new mask from ransac
#
# get ransac result
linePropertyAttribute = findLinearEqOfFieldBoundary( fieldContourMiddle )
# create list of poit
pointList = list()
for lineCoeff in linePropertyAttribute:
# get linear coefficient
x0 = lineCoeff[ 2 ]
xf = lineCoeff[ 3 ]
m = lineCoeff[ 0 ]
c = lineCoeff[ 1 ]
# calculate y from x
x = np.arange( x0, xf, dtype = np.int64 )
y = np.int64( m * x ) + int( c )
contour = np.vstack( ( x, y ) ).transpose()
countour = contour.reshape( -1, 1, 2 )
pointList.append( countour )
if len( pointList ) > 1:
contour = np.vstack( pointList )
# print pointList[ 0 ].shape
# print pointList[ 1 ].shape
# print contour.shape
else:
contour = pointList[ 0 ]
firstPoint = np.array( [ [ [ 0, img.shape[ 0 ] - 1 ] ] ] )
lastPoint = np.array( [ [ [ img.shape[ 1 ] - 1, img.shape[ 0 ] - 1 ] ] ] )
contour = np.concatenate( ( firstPoint, contour, lastPoint ) )
self.contourVis1 = fieldContour
self.contourVis2 = contour
newFieldMask = np.zeros( marker.shape, dtype = np.uint8 )
cv2.drawContours( newFieldMask, [ contour ], 0, 1, -1 )
#
# find white contour in mask
#
# get white object from marker color of white ID is 5
whiteObject = np.zeros( marker.shape, dtype = np.uint8 )
whiteObject[ marker == 5 ] = 1
# get white object only the field
#whiteObjectInField = whiteObject * fieldMask.astype( np.uint8 )
whiteObjectInField = whiteObject * newFieldMask
whiteObjectInField *= 255
# find contour from white object in field
whiteContours = cv2.findContours( whiteObjectInField, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE )[ 1 ]
# extract feature and predict it
extractStatus = self.predictor.extractFeature( img, whiteContours, objectPointLocation = 'bottom' )
# check extract status
if extractStatus == True:
# predict si wait a rai
self.predictor.predict()
bestPositionList = self.predictor.getBestRegion()
if len( bestPositionList ) != 0:
# convert to point32
bestPosition = Point32( bestPositionList[ 0 ], bestPositionList[ 1 ], 0.0 )
# get bounding box object not only position
bestBounding = self.predictor.getBestBoundingBox()
# get another point of object
centerX, centerY = bestBounding.calculateObjectPoint( 'center' )
# calculate error
errorX, errorY = self.calculateError( imageWidth, imageHeight, centerX, centerY )
ballErrorList = Point32( errorX, errorY, 0.0 )
# ball confidence
ballConfidence = 1.0
# define vision message instance
msg = visionMsg()
# assign to message
msg.object_name = [ 'ball' ]
msg.pos2D = [ bestPosition ]
msg.imgH = imageHeight
msg.imgW = imageWidth
msg.object_error = [ ballErrorList ]
msg.object_confidence = [ ballConfidence ]
msg.header.stamp = rospy.Time.now()
rospy.loginfo( "Time usage : {}".format( time.time() - startTime ) )
return msg
def main(self):
rospy.init_node('q_update_client_dqn')
rospy.Subscriber("/state_observation_flag", Int64, self.state_observation_flag_callback)
rospy.Subscriber("/joint1_state", Float64, self.joint1_state_callback)
rospy.Subscriber("/joint3_state", Float64, self.joint3_state_callback)
rospy.Subscriber("/joint5_state", Float64, self.joint5_state_callback)
pub_1 = rospy.Publisher("/joint1_pose", Float64, queue_size = 1)
pub_3 = rospy.Publisher("/joint3_pose", Float64, queue_size = 1)
pub_5 = rospy.Publisher("/joint5_pose", Float64, queue_size = 1)
pub_6 = rospy.Publisher("/action_num", Int64, queue_size = 1)
pub_7 = rospy.Publisher("/num_step", Int64, queue_size = 1)
pub_8 = rospy.Publisher("/num_episode", Int64, queue_size = 1)
pub_9 = rospy.Publisher("/target_point", PointCloud, queue_size = 1)
pub_10 = rospy.Publisher("/vis_target_point", PointCloud, queue_size = 1)
pub_11 = rospy.Publisher("/joint1_pose_com", Float64, queue_size = 1)
pub_13 = rospy.Publisher("/joint3_pose_com", Float64, queue_size = 1)
pub_15 = rospy.Publisher("/joint5_pose_com", Float64, queue_size = 1)
loop_rate = rospy.Rate(100)
filename_result = "/home/amsl/ros_catkin_ws/src/arm_q_learning/dqn_results/test_result.txt"
count = 0
count_temp = 0
self.joint1 = self.init_joint1
self.joint3 = self.init_joint3
self.joint5 = self.init_joint5
print "joint1 : ", self.joint1
print "joint3 : ", self.joint3
print "joint5 : ", self.joint5
self.next_joint1 = self.init_joint1
self.next_joint3 = self.init_joint3
self.next_joint5 = self.init_joint5
self.joint_state[0] = self.init_joint1
self.joint_state[1] = self.init_joint3
self.joint_state[2] = self.init_joint5
print "next joint1 : ", self.next_joint1
print "next joint3 : ", self.next_joint3
print "next joint5 : ", self.next_joint5
step_count = 0
episode_count = 0
episode_now = 0
episode_past = 0
temp_count = 0
loss_list = []
target_vis = PointCloud()
target_vis.header.frame_id = "/base_link"
target_vis.header.stamp = rospy.Time.now()
self.target_point.header.frame_id = "/base_link"
self.target_point.header.stamp = rospy.Time.now()
self.target_point.points.append(Point32(self.target_init_x, self.target_init_y, self.target_init_z))
print type(self.target_point.points[0].x)
print "target_point : ", self.target_point
print "Q Learning Start!!"
x_addition = 0
# print "L : ", self.L
# print "x : ", self.L * math.cos(0.0)+0.270
# rand_target_vis_y = -0.08
pub_11_flag = False
pub_13_flag = False
while not rospy.is_shutdown():
if episode_count == 0:
if step_count == 0:
pub_9.publish(self.target_point)
# rand_target_vis_x = math.sqrt(self.L**2 - rand_target_vis_y**2) + 0.270
# rand_target_vis_z = self.target_init_z
# if rand_target_vis_y <=0.08:
# target_vis.points.append(Point32(rand_target_vis_x, rand_target_vis_y, rand_target_vis_z))
# pub_10.publish(target_vis)
# rand_target_vis_y += 0.01
if self.wait_flag:
# print "wait 10 seconds!!"
count += 1
if count == 1000:
self.wait_flag = False
self.select_action_flag = False
self.q_update_flag = False
self.mode_dqn_flag = True
if episode_count == 0:
self.state_observation_flag = True
self.state_observation_flag1 = True
self.state_observation_flag3 = True
self.state_observation_flag5 = True
pub_11_flag = False
pub_13_flag = False
count = 0
if count == 10:
self.action_num = 0
self.joint5 = self.init_next_joint5
self.next_joint5 = self.init_next_joint5
self.reward = 0.0
pub_1.publish(self.joint1)
pub_3.publish(self.joint3)
pub_5.publish(self.joint5)
pub_6.publish(self.action_num)
print "publish joint5 : ", self.joint5
if count == 100:
self.joint3 = self.init_next_joint3
self.next_joint3 = self.init_next_joint3
pub_1.publish(self.joint1)
pub_3.publish(self.joint3)
pub_5.publish(self.joint5)
pub_6.publish(self.action_num)
print "publish joint3 : ", self.joint3
if count == 300:
self.joint1 = self.init_next_joint1
self.next_joint1 = self.init_next_joint1
pub_1.publish(self.joint1)
pub_3.publish(self.joint3)
pub_5.publish(self.joint5)
pub_6.publish(self.action_num)
print "publish joint1 : ", self.joint1
pub_1.publish(self.joint1)
pub_3.publish(self.joint3)
pub_5.publish(self.joint5)
else:
if self.mode_dqn_flag:
if self.select_action_flag:
self.action = self.epsilon_greedy(self.joint1, self.joint3, self.joint5)
# self.action = 8
self.action_num = self.action
print "self.action_num : ", self.action_num
pub_1.publish(self.joint1)
pub_3.publish(self.joint3)
pub_5.publish(self.joint5)
pub_6.publish(self.action_num)
self.select_action_flag = False
if self.state_observation_flag and self.state_observation_flag1 and self.state_observation_flag3 and self.state_observation_flag5:
print "self.joint_state[0] : ",self.joint_state[0]
print "self.joint_state[1] : ",self.joint_state[1]
print "self.joint_state[2] : ",self.joint_state[2]
print "now joint1 : ", self.joint1
print "now joint3 : ", self.joint3
print "now joint5 : ", self.joint5
self.next_joint1 = self.joint_state[0]
self.next_joint3 = self.joint_state[1]
self.next_joint5 = self.joint_state[2]
print "next joint1 : ", self.next_joint1
print "next joint3 : ", self.next_joint3
print "next joint5 : ", self.next_joint5
self.reward = self.reward_calculation_client(step_count)
print "reward : ", self.reward
# self.select_action_flag = True
self.q_update_flag = True
self.state_observation_flag = False
self.state_observation_flag1 = False
self.state_observation_flag3 = False
self.state_observation_flag5 = False
if self.q_update_flag:
# target_val = self.reward + self.GAMMA * np.max(self.forward(self.next_joint1, self.next_joint3, self.next_joint5).data)
# self.optimizer.zero_grads()
# tt = xp.copy(self.q_list.data)
# tt[0][self.action] = target_val
# target = chainer.Variable(tt)
# loss = 0.5 * (target - self.q_list) ** 2
# loss = F.mean_squared_error(target, self.q_list)
# self.ALPHA = float(self.ALPHA)
# loss.grad = xp.array([[self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA]], dtype=xp.float32)
# loss_list.append(np.max(loss.data))
# loss.backward()
# self.optimizer.update()
self.select_action_flag = True
self.q_update_flag = False
step_count += 1
print "episode : %d " % episode_count,
print "step : %d " % step_count,
print "now joint1 : %d " % self.joint1,
print "now joint3 : %d " % self.joint3,
print "now joint5 : %d " % self.joint5,
print "now action : %d" % self.action,
# print "loss : ", np.max(loss.data),
print "reward : %.1f " % self.reward,
print "EPSILON : %.5f " % self.EPSILON
print ""
# print ""
if self.reward >= 1:
print "episode : %d " % episode_count,
print "step : %d " % step_count,
print "now joint1 : %d " % self.joint1,
print "now joint3 : %d " % self.joint3,
print "now joint5 : %d " % self.joint5,
print "now action : %d" % self.action,
# print "loss average : %.3f " % (sum(loss_list)/len(loss_list)),
print "reward : %.1f " % self.reward,
print "EPSILON : %.5f " % self.EPSILON,
print "succsess!!"
print ""
temp_result = np.array(([[episode_count, step_count]]), dtype=np.int32)
if episode_count == 0:
test_result = temp_result
else:
test_result = np.r_[test_result, temp_result]
# while 1:
# rand_joint1 = randint(0, 1-1)+self.init_joint1
# rand_joint3 = randint(-10, 1)+self.init_joint3
# rand_joint5 = randint(0, 1-1)+self.init_joint5
# if (rand_joint3>=40 and rand_joint3<=47) and (rand_joint1>=-5 and rand_joint1<5):
# print "one more!"
# else:
# self.init_next_joint1 = rand_joint1
# self.init_next_joint3 = rand_joint3
# self.init_next_joint5 = rand_joint5
# break
# while 1:
# rand_target_x = self.target_init_x
# rand_target_y = uniform(self.target_init_y-0.112, self.target_init_y+0.112)
# rand_target_z = uniform(self.target_init_z, self.target_init_z+0.020)
# dz = (0.980 - 0.960)/(0.00 - 0.112)
# if rand_target_y <= 0.0:
# temp_z =-1 * dz * rand_target_y + 0.980
# else:
# temp_z = dz * rand_target_y + 0.980
# if rand_target_z <= temp_z:
# self.target_point.header.stamp = rospy.Time.now()
# self.target_point.points[0].x = rand_target_x
# self.target_point.points[0].y = rand_target_y
# self.target_point.points[0].z = rand_target_z
# self.target_point.points.append(Point32(rand_target_x, rand_target_y, rand_target_z))
# break
# else:
# print "one more!!!"
# rand_target_y = uniform(self.target_init_y-0.08, self.target_init_y+0.08)
rand_target_y = self.target_init_y
# rand_target_x = self.target_init_x
rand_target_x = math.sqrt(self.L**2 - rand_target_y**2) + 0.270
rand_target_z = self.target_init_z
self.target_point.header.stamp = rospy.Time.now()
self.target_point.points[0].x = rand_target_x
self.target_point.points[0].y = rand_target_y
self.target_point.points[0].z = rand_target_z
step_count = 0
episode_count += 1
episode_now = episode_count
# self.action_num = 0
# self.joint1 = self.init_next_joint1
# self.joint3 = self.init_next_joint3
# self.joint5 = self.init_next_joint5
# pub_1.publish(self.joint1)
# pub_3.publish(self.joint3)
# pub_5.publish(self.joint5)
# pub_6.publish(self.action_num)
# loss_list = []
# pub_9.publish(self.target_point)
self.mode_dqn_flag = False
self.state_observation_flag = True
self.state_observation_flag1 = True
self.state_observation_flag3 = True
self.state_observation_flag5 = True
# self.wait_flag = True
else:
if step_count < 50:
self.joint1 = self.next_joint1
self.joint3 = self.next_joint3
self.joint5 = self.next_joint5
episode_past = episode_now
else:
print "episode : %d " % episode_count,
print "step : %d " % step_count,
print "now joint1 : %d " % self.joint1,
print "now joint3 : %d " % self.joint3,
print "now joint5 : %d " % self.joint5,
print "now action : %d" % self.action,
# print "loss average : %.3f " % (sum(loss_list)/len(loss_list)),
print "reward : %.1f " % self.reward,
print "EPSILON : %.5f " % self.EPSILON,
print "failuer!!"
print ""
temp_result = np.array(([[episode_count, step_count]]), dtype=np.int32)
if episode_count == 0:
test_result = temp_result
else:
test_result = np.r_[test_result, temp_result]
# while 1:
# rand_joint1 = randint(0, 1-1)+self.init_joint1
# rand_joint3 = randint(-10, 1)+self.init_joint3
# rand_joint5 = randint(0, 1-1)+self.init_joint5
# if (rand_joint3>=40 and rand_joint3<=47) and (rand_joint1>=-5 and rand_joint1<5):
# print "one more!"
# else:
# self.init_next_joint1 = rand_joint1
# self.init_next_joint3 = rand_joint3
# self.init_next_joint5 = rand_joint5
# break
# while 1:
# rand_target_x = self.target_init_x
# rand_target_y = uniform(self.target_init_y-0.112, self.target_init_y+0.112)
# rand_target_z = uniform(self.target_init_z, self.target_init_z+0.020)
# dz = (0.980 - 0.960)/(0.00 - 0.112)
# if rand_target_y <= 0.0:
# temp_z = -1 * dz * rand_target_y + 0.980
# else:
# temp_z = dz * rand_target_y + 0.980
# if rand_target_z <= temp_z:
# self.target_point.header.stamp = rospy.Time.now()
# self.target_point.points[0].x = rand_target_x
# self.target_point.points[0].y = rand_target_y
# self.target_point.points[0].z = rand_target_z
# self.target_point.points.append(Point32(rand_target_x, rand_target_y, rand_target_z))
# break
# else:
# print "one more!!!"
# rand_target_y = uniform(self.target_init_y-0.08, self.target_init_y+0.08)
rand_target_y = self.target_init_y
# rand_target_x = self.target_init_x
rand_target_x = math.sqrt(self.L**2 - rand_target_y**2) + 0.270
rand_target_z = self.target_init_z
self.target_point.header.stamp = rospy.Time.now()
self.target_point.points[0].x = rand_target_x
self.target_point.points[0].y = rand_target_y
self.target_point.points[0].z = rand_target_z
step_count = 0
episode_count += 1
episode_now = episode_count
self.action_num = 0
# self.joint1 = self.init_next_joint1
# self.joint3 = self.init_next_joint3
# self.joint5 = self.init_next_joint5
# pub_1.publish(self.joint1)
# pub_3.publish(self.joint3)
# pub_5.publish(self.joint5)
# pub_6.publish(self.action_num)
# loss_list = []
# pub_9.publish(self.target_point)
self.mode_dqn_flag = False
self.state_observation_flag = True
self.state_observation_flag1 = True
self.state_observation_flag3 = True
self.state_observation_flag5 = True
# self.wait_flag = True
# if math.fabs(episode_now - episode_past) > 1e-6:
# if self.EPSILON > 0.1000:
# self.EPSILON -= 0.0001
self.num_step = step_count
pub_7.publish(self.num_step)
self.num_episode = episode_count
pub_8.publish(self.num_episode)
# if episode_count%50 == 0:
# model_filename = "/home/amsl/ros_catkin_ws/src/arm_q_learning/dqn_model/dqn_arm_model_%d.dat" % episode_count
# f = open(model_filename, 'w')
# pickle.dump(self.model, f)
# if episode_count > 10000:
# np.savetxt(filename_result, test_result, fmt="%d", delimiter=",")
# f = open('/home/amsl/ros_catkin_ws/src/arm_q_learning/dqn_model/dqn_arm_model.dat', 'w')
# pickle.dump(self.model, f)
# print "Finish!!!"
# break
else:
if self.state_observation_flag and self.state_observation_flag1 and self.state_observation_flag3 and self.state_observation_flag5:
pub_11_flag = False
pub_13_flag = False
self.joint1 = self.next_joint1
self.joint3 = self.next_joint3
self.joint5 = self.next_joint5
print "state_joint1 : ", self.joint1
print "state_joint3 : ", self.joint3
print "state_joint5 : ", self.joint5
print ""
joint1_rad = self.joint1 / 180.0 * self.pi
joint2_rad = 5.0 / 180.0 * self.pi
joint3_rad = self.joint3 / 180.0 * self.pi
joint5_rad = self.joint5 / 180.0 * self.pi
L = self.L3 * math.cos(joint2_rad) + (self.L4 + self.L5) * math.cos(joint2_rad + joint3_rad) + self.L6 * math.cos(joint2_rad + joint3_rad - joint5_rad)
# print "L : ",L
self.arm_end[0] = L * math.cos(joint1_rad)
self.arm_end[1] = L * math.sin(joint1_rad)
self.arm_end[2] = -(self.L1 + self.L2) + self.L3 * math.sin(joint2_rad) + (self.L4 + self.L5) * math.sin(joint2_rad + joint3_rad) + self.L6 * math.sin(joint2_rad + joint3_rad - joint5_rad)
print "end position x : ", self.arm_end[0] + 0.270
print "end position y : ", self.arm_end[1] + 0.000
print "end position z : ", self.arm_end[2] + 0.935
print ""
self.arm_end_com[0] = self.arm_end[0] + x_addition
# self.arm_end_com[0] = self.arm_end[0] + 0.030
self.arm_end_com[1] = self.arm_end[1]
self.arm_end_com[2] = self.arm_end[2]
print "next end position x : ", self.arm_end_com[0] + 0.270
print "next end position y : ", self.arm_end_com[1] + 0.000
print "next end position z : ", self.arm_end_com[2] + 0.935
print ""
self.joint1_com = math.atan2(self.arm_end_com[1], self.arm_end_com[0])
I = math.sqrt(self.arm_end_com[0]**2 + self.arm_end_com[1]**2)
# print "I : ", I
I_L3off = self.L3 * math.cos(joint2_rad)
Z_L3off = self.L3 * math.sin(joint2_rad)
ld = math.sqrt((I - I_L3off)**2 + (self.arm_end_com[2] + (self.L1 + self.L2) - Z_L3off)**2)
# print "ld : ", ld
joint5_com_limit = ((self.L4 + self.L5)**2 + self.L6**2 - ld**2) / (2 * (self.L4 + self.L5) * self.L6)
joint3_com_limit = ((self.L4 + self.L5)**2 + ld**2 - self.L6**2) / (2 * (self.L4 + self.L5) * ld)
if (joint5_com_limit <= 1 and joint3_com_limit <= 1) and x_addition < 0.030:
self.joint5_com =self.pi - math.acos(((self.L4 + self.L5)**2 + self.L6**2 - ld**2) / (2 * (self.L4 + self.L5) * self.L6))
self.joint3_com = math.acos(((self.L4 + self.L5)**2 + ld**2 - self.L6**2) / (2 * (self.L4 + self.L5) * ld)) + math.atan2((self.arm_end_com[2] + (self.L1 + self.L2) - Z_L3off), (I - I_L3off)) - joint2_rad
self.joint1_com = self.joint1_com / self.pi * 180.0
self.joint3_com = self.joint3_com / self.pi * 180.0
self.joint5_com = self.joint5_com / self.pi * 180.0
print "joint1 command : ", self.joint1_com
print "joint3 command : ", self.joint3_com
print "joint5 command : ", self.joint5_com
print ""
# if pub_11_flag:
# pub_11.publish(self.joint1_com)
# pub_11_flag = False
# elif pub_13_flag:
# pub_13.publish(self.joint3_com)
# pub_11_flag = True
# pub_13_flag = False
# else:
# pub_15.publish(self.joint5_com)
# pub_13_flag = True
# print "joint1, joint3, joint5 command publish!!!!"
x_addition += 0.001
print "x add : ", x_addition
else:
x_addition = 0.0
print "can not reach next end position!!!!"
print "this episode finish!!!!!!!!!!!!!!"
self.action_num = 0
# self.joint1 = self.init_next_joint1
# self.joint3 = self.init_next_joint3
# self.joint5 = self.init_next_joint5
pub_1.publish(self.joint1)
pub_3.publish(self.joint3)
pub_5.publish(self.joint5)
pub_6.publish(self.action_num)
loss_list = []
pub_9.publish(self.target_point)
self.wait_flag = True
self.state_observation_flag = False
self.state_observation_flag1 = False
self.state_observation_flag3 = False
self.state_observation_flag5 = False
if pub_11_flag:
pub_11.publish(self.joint1_com)
pub_11_flag = False
print "joint1 command publish!!"
elif pub_13_flag:
pub_13.publish(self.joint3_com)
pub_11_flag = True
pub_13_flag = False
print "joint3 command publish!!"
else:
pub_15.publish(self.joint5_com)
pub_13_flag = True
print "joint5 command publish!!"
# print "joint1, joint3, joint5 command publish!!!!"
loop_rate.sleep()
def processImage(self, image_msg):
now = rospy.Time.now()
if now - self.last_stamp < self.publish_duration:
return
else:
self.last_stamp = now
self.loginfo("Processing image")
vehicle_detected_msg_out = BoolStamped()
vehicle_corners_msg_out = VehicleCorners()
try:
image_cv = self.bridge.compressed_imgmsg_to_cv2(image_msg, "bgr8")
except CvBridgeError as e:
print(e)
# TODO we are only finding a 3x1 circle grid
start = rospy.Time.now()
params = cv2.SimpleBlobDetector_Params()
params.minArea = self.blobdetector_min_area
params.minDistBetweenBlobs = self.blobdetector_min_dist_between_blobs
simple_blob_detector = cv2.SimpleBlobDetector_create(params)
(detection,
corners) = cv2.findCirclesGrid(image_cv, (3, 1),
flags=cv2.CALIB_CB_SYMMETRIC_GRID,
blobDetector=simple_blob_detector)
vehicle_detected_msg_out.data = detection
# if(vehicle_detected_msg_out.data is not False):
# self.loginfo(">>>>>>>>>>>>>>>>>>>>>Published on detection")
self.pub_detection.publish(vehicle_detected_msg_out)
if detection:
self.logwarn("Detected vehicle")
# print(corners)
points_list = []
for point in corners:
corner = Point32()
# print(point[0])
corner.x = point[0, 0]
# print(point[0,1])
corner.y = point[0, 1]
corner.z = 0
points_list.append(corner)
vehicle_corners_msg_out.header.stamp = rospy.Time.now()
vehicle_corners_msg_out.corners = points_list
vehicle_corners_msg_out.detection.data = detection
vehicle_corners_msg_out.H = self.circlepattern_dims[1]
vehicle_corners_msg_out.W = self.circlepattern_dims[0]
self.pub_corners.publish(vehicle_corners_msg_out)
# If we changed detection state
if (self.vehicle_detected != detection):
state = stop_fsm_state if detection else follow_fsm_state
# Set state to vehicle detected -> Stop
request = SetFSMStateRequest(state)
self.loginfo("Detection state to send: " + str(state))
try:
self.setFSMState(request)
except rospy.ServiceException as exc:
rospy.logwarn("FSM service did not process changeState stop:" +
str(exc))
except Exception as e:
rospy.logwarn(
"FSM service did not process changeState, exception: " +
str(e))
except:
rospy.logwarn("FSM service did not process changeState")
self.vehicle_detected = detection
elapsed_time = (rospy.Time.now() - start).to_sec()
self.pub_time_elapsed.publish(elapsed_time)
if self.publish_circles:
cv2.drawChessboardCorners(image_cv, self.circlepattern_dims,
corners, detection)
image_msg_out = self.bridge.cv2_to_imgmsg(image_cv, "bgr8")
self.pub_circlepattern_image.publish(image_msg_out)
return
def base_laser_cb(self, msg):
max_angle = msg.angle_max
ranges = np.array(msg.ranges)
angles = np.arange(msg.angle_min, msg.angle_max, msg.angle_increment)
#Filter out noise(<0.003), points >1m, leaves obstacles
near_angles = np.extract(np.logical_and(ranges < 1, ranges > 0.003),
angles)
near_ranges = np.extract(np.logical_and(ranges < 1, ranges > 0.003),
ranges)
self.bad_side = np.sign(near_angles[np.argmax(abs(near_angles))])
#print "bad side: %s" %bad_side # (1 (pos) = left, -1 = right)
#print "Min in Ranges: %s" %min(ranges)
#if len(near_ranges) > 0:
xs = near_ranges * np.cos(near_angles)
ys = near_ranges * np.sin(near_angles)
#print "xs: %s" %xs
self.points = np.vstack((xs, ys))
#print "Points: %s" %points
self.bfp_points = np.vstack((np.add(0.275, xs), ys))
#print "bfp Points: %s" %bfp_points
self.bfp_dists = np.sqrt(
np.add(np.square(self.bfp_points[0][:]),
np.square(self.bfp_points[1][:])))
#print min(self.bfp_dists)
if len(self.bfp_dists) > 0:
if min(self.bfp_dists) > 0.5:
self.rot_safe = True
else:
self.rot_safe = False
else:
self.rot_safe = True
self.left = np.vstack(
(xs[np.nonzero(ys > 0.35)[0]], ys[np.nonzero(ys > 0.35)[0]]))
self.right = np.vstack(
(xs[np.nonzero(ys < -0.35)[0]], ys[np.nonzero(ys < -0.35)[0]]))
self.front = np.vstack(
(np.extract(np.logical_and(ys < 0.35, ys > -0.35), xs),
np.extract(np.logical_and(ys < 0.35, ys > -0.35), ys)))
front_dist = (self.front[:][0]**2 + self.front[:][1]**2)**(1 / 2)
##Testing and Visualization:###
if len(self.left[:][0]) > 0:
leftScan = PointCloud()
leftScan.header.frame_id = '/base_laser_link'
leftScan.header.stamp = rospy.Time.now()
for i in range(len(self.left[0][:])):
pt = Point32()
pt.x = self.left[0][i]
pt.y = self.left[1][i]
pt.z = 0
leftScan.points.append(pt)
self.left_out.publish(leftScan)
if len(self.right[:][0]) > 0:
rightScan = PointCloud()
rightScan.header.frame_id = '/base_laser_link'
rightScan.header.stamp = rospy.Time.now()
for i in range(len(self.right[:][0])):
pt = Point32()
pt.x = self.right[0][i]
pt.y = self.right[1][i]
pt.z = 0
rightScan.points.append(pt)
self.right_out.publish(rightScan)
if len(self.front[:][0]) > 0:
frontScan = PointCloud()
frontScan.header.frame_id = 'base_laser_link'
frontScan.header.stamp = rospy.Time.now()
for i in range(len(self.front[:][0])):
pt = Point32()
pt.x = self.front[0][i]
pt.y = self.front[1][i]
pt.z = 0
frontScan.points.append(pt)
self.front_out.publish(frontScan)
def main(self):
rospy.init_node('q_update_client_dqn')
rospy.Subscriber("/state_observation_flag", Int64, self.state_observation_flag_callback)
rospy.Subscriber("/joint1_state", Float64, self.joint1_state_callback)
rospy.Subscriber("/joint3_state", Float64, self.joint3_state_callback)
rospy.Subscriber("/joint5_state", Float64, self.joint5_state_callback)
pub_1 = rospy.Publisher("/action_num", Int64, queue_size = 1)
pub_2 = rospy.Publisher("/num_state", Int64, queue_size = 1)
pub_4 = rospy.Publisher("/num_step", Int64, queue_size = 1)
pub_5 = rospy.Publisher("/num_episode", Int64, queue_size = 1)
pub_6 = rospy.Publisher("/target_point", PointCloud, queue_size = 1)
pub_7 = rospy.Publisher("/vis_target_point", PointCloud, queue_size = 1)
loop_rate = rospy.Rate(100)
filename_result = "/home/amsl/ros_catkin_ws/src/arm_q_learning/dqn_results/test_result.txt"
count = 0
count_temp = 0
self.state = self.init_state
self.next_state = self.init_state
self.joint1 = self.state/700
self.joint3 = (self.state%700)/10
self.joint5 = (self.state%10)/1
print "joint1 : ", self.joint1
print "joint3 : ", self.joint3
print "joint5 : ", self.joint5
self.next_joint1 = self.next_state/700
self.next_joint3 = (self.next_state%700)/10
self.next_joint5 = (self.next_state%10)/1
print "next joint1 : ", self.next_joint1
print "next joint3 : ", self.next_joint3
print "next joint5 : ", self.next_joint5
step_count = 0
episode_count = 0
episode_now = 0
episode_past = 0
temp_count = 0
loss_list = []
target_vis = PointCloud()
target_vis.header.frame_id = "/base_link"
target_vis.header.stamp = rospy.Time.now()
self.target_point.header.frame_id = "/base_link"
self.target_point.header.stamp = rospy.Time.now()
self.target_point.points.append(Point32(self.target_init_x, self.target_init_y, self.target_init_z))
print type(self.target_point.points[0].x)
print "target_point : ", self.target_point
print "Q Learning Start!!"
# rand_target_vis_y = -0.08
while not rospy.is_shutdown():
if episode_count == 0:
if step_count == 0:
pub_6.publish(self.target_point)
# rand_target_vis_x = self.target_init_x
# dz = (0.87 - 0.86)/(0.00 + 0.08)
# if rand_target_vis_y <= 0:
# rand_target_vis_z = dz * rand_target_vis_y + 0.87
# else:
# rand_target_vis_z = -1 * dz * rand_target_vis_y + 0.87
# if rand_target_vis_y <=0.08:
# target_vis.points.append(Point32(rand_target_vis_x, rand_target_vis_y, rand_target_vis_z))
# pub_7.publish(target_vis)
# rand_target_vis_y += 0.01
# while 1:
# rand_target_x = self.target_init_x
# rand_target_y = uniform(self.target_init_y-0.08, self.target_init_y+0.08)
# rand_target_z = uniform(self.target_init_z, self.target_init_z+0.03)
# dz = (0.87 - 0.86)/(0.00 + 0.08)
# if rand_target_y <= 0.0:
# temp_z = dz * rand_target_y + 0.87
# else:
# temp_z = -1 * dz * rand_target_y + 0.87
# if rand_target_z <= temp_z:
# self.target_point.points[0].x = rand_target_x
# self.target_point.points[0].y = rand_target_y
# self.target_point.points[0].z = rand_target_z
# self.target_point.points.append(Point32(rand_target_x, rand_target_y, rand_target_z))
# break
# else:
# print "one more!!!"
# pub_6.publish(self.target_point)
if self.wait_flag:
print "wait 1 seconds!!"
count += 1
if count == 100:
self.wait_flag = False
self.select_action_flag = False
self.q_update_flag = False
self.state_observation_flag = True
self.state_observation_flag1 = True
self.state_observation_flag2 = True
self.state_observation_flag3 = True
count = 0
if count == 10:
self.action_num = 0
self.num_state = self.init_next
self.state = self.init_next
self.joint1 = self.state/700
self.joint3 = (self.state%700)/10
self.joint5 = (self.state%10)/1
self.reward = 0.0
pub_1.publish(self.action_num)
pub_2.publish(self.num_state)
else:
if self.select_action_flag:
self.action = self.epsilon_greedy(self.joint1, self.joint3, self.joint5)
self.action_num = self.action
print "self.action_num : ", self.action_num
pub_1.publish(self.action_num)
self.num_state = self.state
pub_2.publish(self.num_state)
self.select_action_flag = False
# print "publish /num_state"
if self.state_observation_flag and self.state_observation_flag1 and self.state_observation_flag2 and self.state_observation_flag3:
self.next_state = self.joint_to_s_client(self.joint_state[0], self.joint_state[1], self.joint_state[2])
print "self.joint_state[0] : ",self.joint_state[0]
print "self.joint_state[1] : ",self.joint_state[1]
print "self.joint_state[2] : ",self.joint_state[2]
print "s = ", self.state
# self.joint1 = self.state/700
# self.joint3 = (self.state%700)/10
# self.joint5 = (self.state%10)/1
print "now joint1 : ", self.joint1
print "now joint3 : ", self.joint3
print "now joint5 : ", self.joint5
print "sd = ", self.next_state
self.next_joint1 = self.next_state/700
self.next_joint3 = (self.next_state%700)/10
self.next_joint5 = (self.next_state%10)/1
print "next joint1 : ", self.next_joint1
print "next joint3 : ", self.next_joint3
print "next joint5 : ", self.next_joint5
self.reward = self.reward_calculation_client(step_count)
print "reward : ", self.reward
# self.select_action_flag = True
self.q_update_flag = True
self.state_observation_flag = False
self.state_observation_flag1 = False
self.state_observation_flag2 = False
self.state_observation_flag3 = False
if self.q_update_flag:
print type(self.forward(self.next_joint1, self.next_joint3, self.next_joint5).data)
target_val = self.reward + self.GAMMA * np.max(self.forward(self.next_joint1, self.next_joint3, self.next_joint5).data)
self.optimizer.zero_grads()
tt = xp.copy(self.q_list.data)
tt[0][self.action] = target_val
target = chainer.Variable(tt)
# loss = 0.5 * (target - self.q_list) ** 2
loss = F.mean_squared_error(target, self.q_list)
# self.ALPHA = float(self.ALPHA)
# loss.grad = xp.array([[self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA, self.ALPHA]], dtype=xp.float32)
loss_list.append(np.max(loss.data))
loss.backward()
self.optimizer.update()
self.select_action_flag = True
self.q_update_flag = False
step_count += 1
print "episode : %d " % episode_count,
print "step : %d " % step_count,
print "now state : %d" % self.state,
print "now action : %d" % self.action,
print "loss : ", np.max(loss.data),
print "reward : %.1f " % self.reward,
print "EPSILON : %.5f " % self.EPSILON
print ""
if self.reward >= 1:
print "episode : %d " % episode_count,
print "step : %d " % step_count,
print "now state : %d" % self.state,
print "now action : %d" % self.action,
print "loss average : %.3f " % (sum(loss_list)/len(loss_list)),
print "reward : %.1f " % self.reward,
print "EPSILON : %.5f " % self.EPSILON,
print "succsess!!"
print ""
temp_result = np.array(([[episode_count, step_count]]), dtype=np.int32)
if episode_count == 0:
test_result = temp_result
else:
test_result = np.r_[test_result, temp_result]
# while 1:
# rand_joint1 = randint(0, 21-1)-10
# rand_joint3 = randint(0, 1-1)+40
# rand_joint5 = randint(0, 1-1)+30
# init_next_temp = (rand_joint1 - self.init_state_joint1) * 700 + (rand_joint3 - self.init_state_joint3) * 10 + (rand_joint5 - self.init_state_joint5) * 1
# print "init_next_temp : ", init_next_temp
# if (rand_joint3>=40 and rand_joint3<=47) and (rand_joint1>=-5 and rand_joint1<5):
# print "one more!!"
# else:
# self.init_next = init_next_temp
# break
while 1:
rand_target_x = self.target_init_x
rand_target_y = uniform(self.target_init_y-0.08, self.target_init_y+0.08)
rand_target_z = uniform(self.target_init_z, self.target_init_z+0.03)
dz = (0.87 - 0.86)/(0.00 + 0.08)
if rand_target_y <= 0.0:
temp_z = dz * rand_target_y + 0.87
else:
temp_z = -1 * dz * rand_target_y + 0.87
if rand_target_z <= temp_z:
self.target_point.header.stamp = rospy.Time.now()
self.target_point.points[0].x = rand_target_x
self.target_point.points[0].y = rand_target_y
self.target_point.points[0].z = rand_target_z
# self.target_point.points.append(Point32(rand_target_x, rand_target_y, rand_target_z))
break
else:
print "one more!!!"
step_count = 0
episode_count += 1
episode_now = episode_count
self.action_num = 0
self.num_state =self. init_next
pub_1.publish(self.action_num)
pub_2.publish(self.num_state)
loss_list = []
pub_6.publish(self.target_point)
self.wait_flag = True
else:
if step_count < 300:
self.state = self.next_state
self.joint1 = self.state/700
self.joint3 = (self.state%700)/10
self.joint5 = (self.state%10)/1
episode_past = episode_now
else:
print "episode : %d " % episode_count,
print "step : %d " % step_count,
print "now state : %d" % self.state,
print "now action : %d" % self.action,
print "loss average : %.3f " % (sum(loss_list)/len(loss_list)),
print "reward : %.1f " % self.reward,
print "EPSILON : %.5f " % self.EPSILON,
print "failuer!!"
print ""
temp_result = np.array(([[episode_count, step_count]]), dtype=np.int32)
if episode_count == 0:
test_result = temp_result
else:
test_result = np.r_[test_result, temp_result]
# while 1:
# rand_joint1 = randint(0, 21-1)-10
# rand_joint3 = randint(0, 1-1)+40
# rand_joint5 = randint(0, 1-1)+30
# init_next_temp = (rand_joint1 - self.init_state_joint1) * 700 + (rand_joint3 - self.init_state_joint3) * 10 + (rand_joint5 - self.init_state_joint5) * 1
# print "init_next_temp : ", init_next_temp
# if (rand_joint3>=40 and rand_joint3<=47) and (rand_joint1>=-5 and rand_joint1<5):
# print "one more!!"
# else:
# self.init_next = init_next_temp
# break
while 1:
rand_target_x = self.target_init_x
rand_target_y = uniform(self.target_init_y-0.08, self.target_init_y+0.08)
rand_target_z = uniform(self.target_init_z, self.target_init_z+0.03)
dz = (0.87 - 0.86)/(0.00 + 0.08)
if rand_target_y <= 0.0:
temp_z = dz * rand_target_y + 0.87
else:
temp_z = -1 * dz * rand_target_y + 0.87
if rand_target_z <= temp_z:
self.target_point.header.stamp = rospy.Time.now()
self.target_point.points[0].x = rand_target_x
self.target_point.points[0].y = rand_target_y
self.target_point.points[0].z = rand_target_z
# self.target_point.points.append(Point32(rand_target_x, rand_target_y, rand_target_z))
break
else:
print "one more!!!"
step_count = 0
episode_count += 1
episode_now = episode_count
self.action_num = 0
self.num_state = self.init_next
pub_1.publish(self.action_num)
pub_2.publish(self.num_state)
loss_list = []
pub_6.publish(self.target_point)
self.wait_flag = True
if math.fabs(episode_now - episode_past) > 1e-6:
if self.EPSILON > 0.1000:
self.EPSILON -= 0.00007
self.num_step = step_count
pub_4.publish(self.num_step)
self.num_episode = episode_count
pub_5.publish(self.num_episode)
if episode_count%50 == 0:
model_filename = "/home/amsl/ros_catkin_ws/src/arm_q_learning/dqn_model/dqn_arm_model_%d.dat" % episode_count
f = open(model_filename, 'w')
pickle.dump(self.model, f)
if episode_count > 20000:
np.savetxt(filename_result, test_result, fmt="%d", delimiter=",")
# f = open('/home/amsl/ros_catkin_ws/src/arm_q_learning/dqn_model/dqn_arm_model.dat', 'w')
# pickle.dump(self.model, f)
print "Finish!!!"
break
loop_rate.sleep()
def run_grasp_planning():
# The transformation matrix
camera2world = quaternion_matrix([
0.13363039718756375, -0.6601503565555793, 0.7226078483153184,
-0.1555066597935349
])
camera2world[0, 3] = 0.29972335333107686
camera2world[1, 3] = -0.00016958877060524544
camera2world[2, 3] = 0.8278206244574912
# The keypoint in camera frame
keypoint_camera = np.zeros((3, 3))
keypoint_camera[0, :] = np.array(
[-0.0710507483878, -0.0075068516829, 0.909739379883])
keypoint_camera[1, :] = np.array(
[-0.0641933829504, 0.0298324972555, 0.838331695557])
keypoint_camera[2, :] = np.array(
[-0.0684536122998, -0.0403231180828, 0.821209274292])
# Transformed into world
keypoint_world = np.zeros_like(keypoint_camera)
for i in range(3):
keypoint_world[i, :] = camera2world[0:3, 0:3].dot(
keypoint_camera[i, :]) + camera2world[0:3, 3]
# Read the point cloud
project_path = os.path.join(os.path.dirname(os.path.realpath(__file__)),
os.path.pardir)
project_path = os.path.abspath(project_path)
pcdpath = os.path.join(project_path,
'kplan/keypoint_grasp/data/completed_shape.pcd')
pcd = open3d.read_point_cloud(pcdpath)
pc_camera_frame = np.asarray(pcd.points)
pc_world_frame = camera2world[0:3, 0:3].dot(pc_camera_frame.T)
pc_world_frame[0, :] = pc_world_frame[0, :] + camera2world[0, 3]
pc_world_frame[1, :] = pc_world_frame[1, :] + camera2world[1, 3]
pc_world_frame[2, :] = pc_world_frame[2, :] + camera2world[2, 3]
pc_world_frame = pc_world_frame.T
# Construct the request
request = KeypointGraspingServiceRequest()
# The camera pose
request.camera2world.orientation.w = 0.13363039718756375
request.camera2world.orientation.x = -0.6601503565555793
request.camera2world.orientation.y = 0.7226078483153184
request.camera2world.orientation.z = -0.1555066597935349
request.camera2world.position.x = 0.29972335333107686
request.camera2world.position.y = -0.00016958877060524544
request.camera2world.position.z = 0.8278206244574912
# The keypoint
point_0, point_1, point_2 = Point(), Point(), Point()
point_0.x, point_0.y, point_0.z = keypoint_world[0, 0], keypoint_world[
0, 1], keypoint_world[0, 2]
point_1.x, point_1.y, point_1.z = keypoint_world[1, 0], keypoint_world[
1, 1], keypoint_world[1, 2]
point_2.x, point_2.y, point_2.z = keypoint_world[2, 0], keypoint_world[
2, 1], keypoint_world[2, 2]
request.keypoint_world_frame.append(point_0)
request.keypoint_world_frame.append(point_1)
request.keypoint_world_frame.append(point_2)
# The point cloud
request.pointcloud_world_frame = []
for i in range(pc_world_frame.shape[0]):
point_i = Point32()
point_i.x = pc_world_frame[i, 0]
point_i.x = pc_world_frame[i, 1]
point_i.x = pc_world_frame[i, 2]
request.pointcloud_world_frame.append(point_i)
# Construct the service
rospy.wait_for_service('plan_grasp')
plan_grasp = rospy.ServiceProxy('plan_grasp', KeypointGraspingService)
# Call the service
response = plan_grasp(request) # type: KeypointGraspingServiceResponse
# To 4x4 matrix
quat_fingertip_in_world = [
response.gripper_fingertip_in_world.orientation.w,
response.gripper_fingertip_in_world.orientation.x,
response.gripper_fingertip_in_world.orientation.y,
response.gripper_fingertip_in_world.orientation.z
]
T_fingertip_in_world = quaternion_matrix(quat_fingertip_in_world)
T_fingertip_in_world[0, 3] = response.gripper_fingertip_in_world.position.x
T_fingertip_in_world[1, 3] = response.gripper_fingertip_in_world.position.y
T_fingertip_in_world[2, 3] = response.gripper_fingertip_in_world.position.z
# visualize it
from kplan.visualizer.meshcat_wrapper import MeshCatVisualizer
visualizer = MeshCatVisualizer()
visualizer.add_frame(T_fingertip_in_world, frame_scale=0.1)
visualizer.add_pointcloud(pc_world_frame)
if self.takeoffService(altitude=3):
return True
else:
print("Vechile Takeoff failed")
return False
def sat(self, a, maxv):
n = np.linalg.norm(a)
if n > maxv:
return a/n*maxv
else:
return a
def bias_cb(msg):
global feb_pos
feb_pos.x = msg.x
feb_pos.y = msg.y
feb_pos.z = msg.z
if __name__ == '__main__':
rospy.init_node('decision_node', anonymous=True)
feb_pos = Point32()
param_id = rospy.get_param("~drone_id")
param_num = rospy.get_param("~drone_num")
bias_pos_sub = rospy.Subscriber('/drone_%s/mavros/local_position/pose_cor'%(param_id), Point32, bias_cb)
px4 = Px4Controller(param_id,param_num,feb_pos)
px4.start()
rospy.spin()
print("Finish")
def kinematicCalculation( self, objMsg, joint ):
# get object name and passthrough
objectNameList = objMsg.object_name
# Get camera kinematics
# transformationMatrix = self.forwardKinematics( joint )
qPan = getJsPosFromName( joint, "pan" )
qTilt = getJsPosFromName( joint, "tilt" )
transformationMatrix = getMatrixForForwardKinematic( qPan, qTilt )
# initial list
ball3DCartesianList = list()
ball2DPolarList = list()
#
# convert for 'ball' only
#
# loop every object to calculate 3D position
for objIndex in range( len( objMsg.object_name ) ):
if objMsg.object_confidence[ objIndex ] > 0.50:
# get 2D ball position
ballPosition = np.array( [ objMsg.pos2D[ objIndex ].x,
objMsg.pos2D[ objIndex ].y ], dtype = np.float64 )
ballPosition = ballPosition.reshape( -1, 2 )
# get ball in world coordinate
# ball3DCartesian is returned in form [ ( 'projection', arrayOfPos ) ]
ball3DCartesian = self.calculate3DCoor( ballPosition,
HCamera = transformationMatrix )[ 0 ][ 1 ]
# get polar coordinate
# handle error when 3D coordinate is cannot calculate 3D position
try:
r = np.sqrt( ball3DCartesian[ 0 ] ** 2 + ball3DCartesian[ 1 ] ** 2 )
theta = np.arctan2( ball3DCartesian[ 1 ], ball3DCartesian[ 0 ] )
ball2DPolar = np.array( [ r, theta ] )
# compress to msg
ball3DCartesianMsg = Point32( ball3DCartesian[ 0 ],
ball3DCartesian[ 1 ],
ball3DCartesian[ 2 ] )
ball2DPolarMsg = Point32( ball2DPolar[ 0 ],
ball2DPolar[ 1 ],
0 )
except Exception as e:
rospy.logwarn( e )
ball3DCartesianMsg = Point32()
ball2DPolarMsg = Point32()
else:
ball3DCartesianMsg = Point32()
ball2DPolarMsg = Point32()
ball3DCartesianList.append( ball3DCartesianMsg )
ball2DPolarList.append( ball2DPolarMsg )
# If not find the ball
# if objMsg.ball_confidence == False or len( objMsg.ball ) == 0:
#
# # Set ball 3D Cartesion is None
# ball3DCartesian = None
#
# else:
# # Calculate 3D coordinate respect to base frame
# ballPosition = np.array( objMsg.ball, dtype = np.float64 ).reshape( -1, 2 )
# ball3DCartesian = self.calculate3DCoor( ballPosition, HCamera = transformationMatrix )
# ball3DCartesian = ball3DCartesian[ 0 ][ 1 ]
#
# # If ball 3D cartesion is None
# if ball3DCartesian is not None:
#
# # Calculate polar coordinate change x, y -> r, theta
# #ball2DPolar = cv2.cartToPolar( ball3DCartesian[ 0 ], ball3DCartesian[ 1 ] )
# #ball2DPolar = np.array( [ ball2DPolar[ 0 ][ 0 ], ball2DPolar[ 0 ][ 0 ] ] )
# r = np.sqrt( ball3DCartesian[ 0 ] ** 2 + ball3DCartesian[ 1 ] ** 2 )
# theta = np.arctan2( ball3DCartesian[ 1 ], ball3DCartesian[ 0 ] )
# ball2DPolar = np.array( [ r, theta ] )
#
# else:
#
# # If can't calculate 3D return empty vector
# ball3DCartesian = np.array( [ ] )
# ball2DPolar = np.array( [ ] )
#
# rospy.loginfo( "\nPosition in 3D coordinate : {}\n".format( ball3DCartesian ) )
# rospy.logdebug( "\nPosition in Polar coordinate : {}\n".format( ball2DPolar ) )
# Get 2D projection back to camera
self.point2D1 = self.calculate2DCoor( self.points, "ground", HCamera= transformationMatrix )
self.point2D1 = np.array( self.point2D1 )
# Publist positon dict message
msg = postDictMsg()
msg.object_name = objectNameList
msg.pos3D_cart = ball3DCartesianList
msg.pos2D_polar = ball2DPolarList
msg.pos2D = objMsg.pos2D
msg.imgW = objMsg.imgW
msg.imgH = objMsg.imgH
msg.object_error = objMsg.object_error
msg.object_confidence = objMsg.object_confidence
msg.header.stamp = rospy.Time.now()
return msg
def poly_to_ros(p):
newpoly = RosPolygon()
newpoly.points = [Point32(x, y, 0) for x, y in p]
return newpoly
def callback(self, point_cloud):
print("callback")
points = np.array(list(pc2.read_points(point_cloud, skip_nans=True)))
z_points = points[:, [2]]
self.max_z = max(z_points)
points_surface = points[:, [0, 1]] # choose x, y
points_surface = points_surface[np.argsort(
points_surface[:, 0])] # sort x
min_x = points_surface[0, 0]
max_x = points_surface[-1, 0]
points_surface = points_surface[np.argsort(
points_surface[:, 1])] # sort x
min_y = points_surface[0, 1]
max_y = points_surface[-1, 1]
self.image_pixel_zero_pos = [min_x, min_y]
pixel_size_x = ((max_x - min_x) / self.one_pixel_length)
pixel_size_y = ((max_y - min_y) / self.one_pixel_length)
pixel_size_x_int = int((max_x - min_x) / self.one_pixel_length)
pixel_size_y_int = int((max_y - min_y) / self.one_pixel_length)
if ((pixel_size_x - pixel_size_x_int) >= 0.5):
pixel_size_x_int = pixel_size_x_int + 1
if ((pixel_size_y - pixel_size_y_int) >= 0.5):
pixel_size_y_int = pixel_size_y_int + 1
np_image = np.zeros((pixel_size_x_int, pixel_size_y_int, 3))
print("image size:", pixel_size_x_int, pixel_size_y_int)
for i in points_surface:
x = i[0]
y = i[1]
pix_x = (x - self.image_pixel_zero_pos[0]) / self.one_pixel_length
pix_y = (y - self.image_pixel_zero_pos[1]) / self.one_pixel_length
pix_x_int = int(
(x - self.image_pixel_zero_pos[0]) / self.one_pixel_length)
pix_y_int = int(
(y - self.image_pixel_zero_pos[1]) / self.one_pixel_length)
if (pix_x - pix_x_int >= 0.5):
pix_x_int = pix_x_int + 1
if (pix_y - pix_y_int >= 0.5):
pix_y_int = pix_y_int + 1
np_image[pix_x_int - 1][pix_y_int - 1] = [255, 255, 255]
img = np_image.astype(np.uint8)
kernel = np.ones((5, 5), np.uint8)
img = cv2.morphologyEx(img, cv2.MORPH_CLOSE, kernel)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
_, bw = cv2.threshold(gray, 0, 255,
cv2.THRESH_BINARY | cv2.THRESH_OTSU)
ret = findSquares(bw, img)
img_polygons = ret[0]
img_centers = ret[1]
polygons = []
centers = []
for i in range(len(img_polygons)):
polygon = []
for j in range(len(img_polygons[0])):
pos_xy = self.pixel_to_pos(img_polygons[i][j])
polygon.append([pos_xy[0], pos_xy[1], self.max_z])
polygons.append(polygon)
pos_xy = self.pixel_to_pos(img_centers[i])
centers.append([pos_xy[0], pos_xy[1], self.max_z])
header = point_cloud.header
msg = PolygonArray()
msg.header = header
for i in range(len(polygons)):
p = PolygonStamped()
p.header = header
for j in range(len(polygons[0])):
xyz = polygons[i][j]
p.polygon.points.append(Point32(x=xyz[0], y=xyz[1], z=xyz[2]))
msg.polygons.append(p)
self.pub_polygons.publish(msg)
pub_msg = PoseArray()
pub_msg.header = point_cloud.header
for i in range(len(centers)):
pose = Pose()
pose.position.x = centers[i][0]
pose.position.y = centers[i][1]
pose.position.z = centers[i][2]
pub_msg.poses.append(pose)
self.pub_target_poses.publish(pub_msg)
def main(self):
rospy.init_node('q_update_client_tis')
rospy.Subscriber("/state_observation_flag", Int64,
self.state_observation_flag_callback)
rospy.Subscriber("/joint1_state", Float64, self.joint1_state_callback)
rospy.Subscriber("/joint2_state", Float64, self.joint2_state_callback)
rospy.Subscriber("/joint3_state", Float64, self.joint3_state_callback)
pub_1 = rospy.Publisher("/joint1_pose", Float64, queue_size=1)
pub_2 = rospy.Publisher("/joint2_pose", Float64, queue_size=1)
pub_3 = rospy.Publisher("/joint3_pose", Float64, queue_size=1)
pub_6 = rospy.Publisher("/action_num", Int64, queue_size=1)
pub_7 = rospy.Publisher("/num_step", Int64, queue_size=1)
pub_8 = rospy.Publisher("/num_episode", Int64, queue_size=1)
pub_9 = rospy.Publisher("/target_point", PointCloud, queue_size=1)
pub_10 = rospy.Publisher("/vis_target_point", PointCloud, queue_size=1)
loop_rate = rospy.Rate(100)
count = 0
count_temp = 0
print "joint1 : ", self.joint1
print "joint2 : ", self.joint2
print "joint3 : ", self.joint3
print "next joint1 : ", self.next_joint1
print "next joint2 : ", self.next_joint2
print "next joint3 : ", self.next_joint3
step_count = 0
episode_count = 0
episode_now = 0
episode_past = 0
temp_count = 0
loss_list = []
target_vis = PointCloud()
target_vis.header.frame_id = "/base_link"
target_vis.header.stamp = rospy.Time.now()
self.target_point.header.frame_id = "/base_link"
self.target_point.header.stamp = rospy.Time.now()
self.target_point.points.append(
Point32(self.target_init_x, self.target_init_y,
self.target_init_z))
print type(self.target_point.points[0].x)
print "target_point : ", self.target_point
print "Q Learning Start!!"
# print "L_1 : ", self.L_1
# print "L_2 : ", self.L_2
# print "x : ", self.L_1 * math.cos(0.0)+0.270
# rand_target_vis_y = -0.08
while not rospy.is_shutdown():
if episode_count == 0:
if step_count == 0:
pub_9.publish(self.target_point)
# if count == 0:
# rand_target_vis_x = math.sqrt(self.L_1**2 - rand_target_vis_y**2) + 0.270
# elif count == 1:
# rand_target_vis_x = math.sqrt(self.L_2**2 - rand_target_vis_y**2) + 0.270
# rand_target_vis_z = self.target_init_z
# rand_target_vis_y = uniform(self.target_init_y-0.06, self.target_init_y+0.06)
# rand_target_x = self.target_init_x
# rand_target_vis_x = uniform(math.sqrt(self.L_2**2 - rand_target_vis_y**2) + 0.270, math.sqrt(self.L_1**2 - rand_target_vis_y**2) + 0.270)
# rand_target_vis_z = self.target_init_z
# if rand_target_vis_y <=0.08:
# target_vis.points.append(Point32(rand_target_vis_x, rand_target_vis_y, rand_target_vis_z))
# rospy.sleep(0.5)
# pub_10.publish(target_vis)
# rand_target_vis_y += 0.01
# if count == 0:
# rand_target_vis_y += 0.01
# if count == 1:
# rand_target_vis_y -= 0.01
# if rand_target_vis_y == 0.08:
# count += 1
# if rand_target_vis_y == -0.08:
# count += 1
if self.wait_flag:
print "wait 1 seconds!!"
count += 1
if count == 100:
self.wait_flag = False
self.select_action_flag = False
self.q_update_flag = False
self.state_observation_flag = True
self.state_observation_flag1 = True
self.state_observation_flag2 = True
self.state_observation_flag3 = True
count = 0
if count == 50:
self.action_num = 0
self.joint1 = self.init_next_joint1
self.joint2 = self.init_next_joint2
self.joint3 = self.init_next_joint3
self.reward = 0.0
pub_1.publish(self.joint1)
pub_2.publish(self.joint2)
pub_3.publish(self.joint3)
pub_6.publish(self.action_num)
else:
if self.select_action_flag:
step_count += 1
self.action = self.epsilon_greedy(self.joint1, self.joint2,
self.joint3)
self.action_num = self.action
print "self.action_num : ", self.action_num
pub_1.publish(self.joint1)
pub_2.publish(self.joint2)
pub_3.publish(self.joint3)
pub_6.publish(self.action_num)
self.select_action_flag = False
if self.state_observation_flag and self.state_observation_flag1 and self.state_observation_flag2 and self.state_observation_flag3:
print "self.joint_state[0] : ", self.joint_state[0]
print "self.joint_state[1] : ", self.joint_state[1]
print "self.joint_state[2] : ", self.joint_state[2]
print "now joint1 : ", self.joint1
print "now joint2 : ", self.joint2
print "now joint3 : ", self.joint3
self.next_joint1 = self.joint_state[0]
self.next_joint2 = self.joint_state[1]
self.next_joint3 = self.joint_state[2]
print "next joint1 : ", self.next_joint1
print "next joint2 : ", self.next_joint2
print "next joint3 : ", self.next_joint3
if step_count == 0:
self.select_action_flag = True
else:
self.reward = self.reward_calculation_client(
step_count)
print "reward : ", self.reward
self.q_update_flag = True
self.state_observation_flag = False
self.state_observation_flag1 = False
self.state_observation_flag2 = False
self.state_observation_flag3 = False
if self.q_update_flag:
target_val = self.reward + self.GAMMA * np.max(
self.forward(self.next_joint1, self.next_joint2,
self.next_joint3).data)
self.optimizer.zero_grads()
tt = xp.copy(self.q_list.data)
tt[0][self.action] = target_val
target = chainer.Variable(tt)
# loss = 0.5 * (target - self.q_list) ** 2
loss = F.mean_squared_error(target, self.q_list)
loss_list.append(np.max(loss.data))
loss.backward()
self.optimizer.update()
self.reward_flag = True
self.select_action_flag = True
self.q_update_flag = False
print "episode : %d " % episode_count,
print "step : %d " % step_count,
print "now joint1 : %d " % self.joint1,
print "now joint2 : %d " % self.joint2,
print "now joint3 : %d " % self.joint3,
print "now action : %d" % self.action,
print "loss : ", np.max(loss.data),
print "reward : %.1f " % self.reward,
print "EPSILON : %.5f " % self.EPSILON
print ""
if self.reward_flag:
self.reward_flag = False
if self.reward >= 1:
print "episode : %d " % episode_count,
print "step : %d " % step_count,
print "now joint1 : %d " % self.joint1,
print "now joint2 : %d " % self.joint2,
print "now joint3 : %d " % self.joint3,
print "now action : %d" % self.action,
print "loss average : %.3f " % (sum(loss_list) /
len(loss_list)),
print "reward : %.1f " % self.reward,
print "EPSILON : %.5f " % self.EPSILON,
print "succsess!!"
print ""
temp_result = np.array(([[episode_count, step_count]]),
dtype=np.int32)
if episode_count == 0:
test_result = temp_result
else:
test_result = np.r_[test_result, temp_result]
if episode_count % 100 == 0:
model_filename = "/home/amsl/ros_catkin_ws/src/arm_q_learning/dqn_model/dqn_arm_model_%d.dat" % episode_count
f = open(model_filename, 'w')
pickle.dump(self.model, f)
filename_result1 = "/home/amsl/ros_catkin_ws/src/arm_q_learning/dqn_results/test_result_%d.txt" % episode_count
np.savetxt(filename_result1,
test_result,
fmt="%d",
delimiter=",")
rand_target_y = uniform(self.target_init_y - 0.06,
self.target_init_y + 0.06)
rand_target_x = math.sqrt(self.L_2**2 -
rand_target_y**2) + 0.270
# rand_target_x = uniform(math.sqrt(self.L_2**2 - rand_target_y**2) + 0.270, math.sqrt(self.L_1**2 - rand_target_y**2) + 0.270)
rand_target_z = self.target_init_z
self.target_point.header.stamp = rospy.Time.now()
self.target_point.points[0].x = rand_target_x
self.target_point.points[0].y = rand_target_y
self.target_point.points[0].z = rand_target_z
step_count = 0
episode_count += 1
episode_now = episode_count
self.action_num = 0
self.joint1 = self.init_next_joint1
self.joint2 = self.init_next_joint2
self.joint3 = self.init_next_joint3
pub_1.publish(self.joint1)
pub_2.publish(self.joint2)
pub_3.publish(self.joint3)
pub_6.publish(self.action_num)
loss_list = []
pub_9.publish(self.target_point)
self.wait_flag = True
else:
if step_count < 300:
self.joint1 = self.next_joint1
self.joint2 = self.next_joint2
self.joint3 = self.next_joint3
episode_past = episode_now
else:
print "episode : %d " % episode_count,
print "step : %d " % step_count,
print "now joint1 : %d " % self.joint1,
print "now joint2 : %d " % self.joint2,
print "now joint3 : %d " % self.joint3,
print "now action : %d" % self.action,
print "loss average : %.3f " % (sum(loss_list) /
len(loss_list)),
print "reward : %.1f " % self.reward,
print "EPSILON : %.5f " % self.EPSILON,
print "failuer!!"
print ""
temp_result = np.array(
([[episode_count, step_count]]),
dtype=np.int32)
if episode_count == 0:
test_result = temp_result
else:
test_result = np.r_[test_result, temp_result]
if episode_count % 100 == 0:
model_filename = "/home/amsl/ros_catkin_ws/src/arm_q_learning/dqn_model/dqn_arm_model_%d.dat" % episode_count
f = open(model_filename, 'w')
pickle.dump(self.model, f)
filename_result1 = "/home/amsl/ros_catkin_ws/src/arm_q_learning/dqn_results/test_result_%d.txt" % episode_count
np.savetxt(filename_result1,
test_result,
fmt="%d",
delimiter=",")
rand_target_y = uniform(self.target_init_y - 0.06,
self.target_init_y + 0.06)
rand_target_x = math.sqrt(self.L_2**2 -
rand_target_y**2) + 0.270
# rand_target_x = uniform(math.sqrt(self.L_2**2 - rand_target_y**2) + 0.270, math.sqrt(self.L_1**2 - rand_target_y**2) + 0.270)
rand_target_z = self.target_init_z
self.target_point.header.stamp = rospy.Time.now()
self.target_point.points[0].x = rand_target_x
self.target_point.points[0].y = rand_target_y
self.target_point.points[0].z = rand_target_z
step_count = 0
episode_count += 1
episode_now = episode_count
self.action_num = 0
self.joint1 = self.init_next_joint1
self.joint2 = self.init_next_joint2
self.joint3 = self.init_next_joint3
pub_1.publish(self.joint1)
pub_2.publish(self.joint2)
pub_3.publish(self.joint3)
pub_6.publish(self.action_num)
loss_list = []
pub_9.publish(self.target_point)
self.wait_flag = True
if math.fabs(episode_now - episode_past) > 1e-6:
if self.EPSILON > 0.1000:
self.EPSILON -= 0.000025
self.num_step = step_count
pub_7.publish(self.num_step)
self.num_episode = episode_count
pub_8.publish(self.num_episode)
if episode_count > 40000:
filename_result = "/home/amsl/ros_catkin_ws/src/arm_q_learning/dqn_results/test_result.txt"
np.savetxt(filename_result,
test_result,
fmt="%d",
delimiter=",")
print "Finish!!!"
break
loop_rate.sleep()
from math import * import numpy as np from tf.transformations import euler_from_quaternion import rospy from geometry_msgs.msg import Twist from geometry_msgs.msg import Point32 from vicon.msg import Subject import time from robot_drawer import RobotDrawer import pickle from shapely.geometry import Polygon, LinearRing, Point, LineString from numpy import linalg as LA goal = Point32() goal.x = 0.0000001 goal.y = 0.0000001 # state tracking = False new_tracking_msg = False # P Control constants for navigation p_linear = .1 p_angular = .1 P_TRACKING = 0.70 pose = None import matplotlib
def getMsg(msg):
gen = point_cloud2.read_points(msg, skip_nans=True)
# print(type(gen))
cnt = 0
points_list = []
for p in gen:
# if p[4] is 7:
points_list.append([p[0], p[1], p[2], p[3]])
pcl_data = pcl.PointCloud_PointXYZRGB()
#pcl_data = points_list
pcl_data.from_list(points_list)
# downsampling 실행 코드 부분
print("Input :", pcl_data)
LEAF_SIZE = 0.001
cloud = do_voxel_grid_downssampling(pcl_data, LEAF_SIZE)
print("")
# ROI 실행 코드 부분
filter_axis = 'x'
axis_min = 0
axis_max = 10
cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max)
filter_axis = 'y'
axis_min = -2
axis_max = 2
cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max)
filter_axis = 'z'
axis_min = -0.3
axis_max = 1
cloud = do_passthrough(cloud, filter_axis, axis_min, axis_max)
print("Output :", cloud)
test = PointCloud()
get_in = ChannelFloat32()
get_in.name = 'intensery'
test.points = []
for p in cloud:
# if p[1] > 0:
seo = Point32()
seo.x = p[0]
seo.y = p[1]
seo.z = p[2]
get_in.values.append(p[3])
test.points.append(seo)
cnt += 1
# print(p[3])
# rate = rospy.Rate(10)
#print("Input :", cnt)
test.channels.append(get_in)
test.header.frame_id = 'world'
test.header.stamp = rospy.Time.now()
#test.channels = 3
# print(test)
pub.publish(test)
def get_new_point32(self):
from geometry_msgs.msg import Point32
return Point32(1.23, 4.56, 7.89)
def ImageProcessingFunction(self, img, header):
startTime = time.time()
objNameList = list()
pos2DList = list()
errorList = list()
confidenceList = list()
imageWidth = img.shape[1]
imageHeight = img.shape[0]
blurImage = cv2.GaussianBlur(img, (5, 5), 0)
hsvImage = cv2.cvtColor(blurImage, cv2.COLOR_BGR2HSV)
marker = colorSegmentation(blurImage, self.colorDefList)
marker = cv2.watershed(hsvImage, marker)
fieldContour, fieldMask = findBoundary(marker, 2)
ransac = findNewLineFromRansac(fieldContour, imageWidth, imageHeight)
if len(ransac) > 0:
ransacContours, coeff = ransac[0], ransac[1]
else:
ransacContours, coeff = fieldContour, []
if len(coeff) > 1:
# find y intersect
xIntersect = coeff[0][3]
m = coeff[0][0]
c = coeff[0][1]
yIntersect = (m * xIntersect) + c
intersectPoint = Point32(x=xIntersect, y=yIntersect, z=0.0)
errorX, errorY = self.calculateError(imageWidth, imageHeight,
xIntersect, yIntersect)
errorIntersectPoint = Point32(x=errorX, y=errorY, z=0.0)
objNameList.append('field_corner')
pos2DList.append(intersectPoint)
errorList.append(errorIntersectPoint)
confidenceList.append(1.0)
newFieldMask = np.zeros(marker.shape, dtype=np.uint8)
cv2.drawContours(newFieldMask, [ransacContours], 0, 1, -1)
whiteObjectMask = np.zeros(marker.shape, dtype=np.uint8)
whiteObjectMask[marker == 5] = 1
whiteObjectInFieldMask = whiteObjectMask * newFieldMask * 255
kernel = np.ones((5, 5), dtype=np.uint8)
whiteObjectInFieldMask = cv2.morphologyEx(whiteObjectInFieldMask,
cv2.MORPH_OPEN, kernel)
whiteObjectContours = cv2.findContours(whiteObjectInFieldMask,
cv2.RETR_TREE,
cv2.CHAIN_APPROX_SIMPLE)[1]
canExtract = self.predictor.extractFeature(
img, whiteObjectContours, objectPointLocation="bottom")
if canExtract:
self.predictor.predict()
goalList = self.predictor.getGoal()
bestPosition = tuple(self.predictor.getBestRegion())
if len(bestPosition) != 0:
objNameList.append('ball')
# get bounding box object not only position
bestBounding = self.predictor.getBestBoundingBox()
# get another point of object
centerX, centerY = bestBounding.calculateObjectPoint('center')
pos2DList.append(
Point32(bestPosition[0][0], bestPosition[0][1], 0.0))
# calculate error
errorX, errorY = self.calculateError(imageWidth, imageHeight,
centerX, centerY)
errorList.append(Point32(errorX, errorY, 0.0))
confidenceList.append(bestPosition[1])
for goal in goalList:
objNameList.append('goal')
pos2DList.append(Point32(goal[0][0], goal[0][1], 0.0))
errorX, errorY = self.calculateError(imageWidth, imageHeight,
goal[0][0], goal[0][1])
errorList.append(Point32(errorX, errorY, 0.0))
confidenceList.append(goal[1])
poles = findPole(marker,
self.orangeID,
self.blueID,
self.yellowID,
self.magentaID,
mask=fieldMask)
for n, center in poles.items():
for x, y in center:
objNameList.append(n)
pos2DList.append(Point32(x=x, y=y))
errorX, errorY = self.calculateError(imageWidth, imageHeight,
x, y)
errorList.append(Point32(x=errorX, y=errorY))
confidenceList.append(1.0)
msg = self.createVisionMsg(objNameList, pos2DList, errorList,
confidenceList, imageWidth, imageHeight)
rospy.logdebug("Time usage : {}".format(time.time() - startTime))
return msg
pub_vel_LRW = rospy.Publisher(LRW_topic, Float64, queue_size=1) pub_vel_RRW = rospy.Publisher(RRW_topic, Float64, queue_size=1) pub_vel_LFW = rospy.Publisher(LFW_topic, Float64, queue_size=1) pub_vel_RFW = rospy.Publisher(RFW_topic, Float64, queue_size=1) pub_pos_LSH = rospy.Publisher(LSH_topic, Float64, queue_size=1) pub_pos_RSH = rospy.Publisher(RSH_topic, Float64, queue_size=1) # footprint parameters global seq seq = 0 footprint = PolygonStamped() side_A = Point32() side_B = Point32() side_C = Point32() side_D = Point32() side_E = Point32() [side_A.x, side_A.y, side_A.z] = [-0.1, -0.2, 0.0] [side_B.x, side_B.y, side_B.z] = [0.5, -0.2, 0.0] [side_C.x, side_C.y, side_C.z] = [0.6, 0.0, 0.0] [side_D.x, side_D.y, side_D.z] = [0.5, 0.2, 0.0] [side_E.x, side_E.y, side_E.z] = [-0.1, 0.2, 0.0] footprint.header.frame_id = base_frame footprint.polygon.points = [side_A, side_B, side_C, side_D, side_E] # footprint visualizer
def spoofPointCloud():
rospy.init_node('point_spoof', anonymous=True)
pub_pointcloud = rospy.Publisher('point_cloud', PointCloud, queue_size=1)
rate = rospy.Rate(10) # 10hz
targetsPointCloud = PointCloud()
targetsPointCloud.header.frame_id = "world"
#extra1a=Point32()
#extra1a.x=-124.6
#extra1a.y=14.95
#extra1a.z=.2
#targetsPointCloud.points.append(extra1a)
#extra1b=Point32()
#extra1b.x=-125
#extra1b.y=15
#extra1b.z=.2
#targetsPointCloud.points.append(extra1b)
blueA = Point32()
blueA.x = -90.68
blueA.y = 16.7
blueA.z = .2
targetsPointCloud.points.append(blueA)
blueB = Point32()
blueB.x = -90.7
blueB.y = 16.71
blueB.z = .2
targetsPointCloud.points.append(blueB)
extra1a = Point32()
extra1a.x = -86.61
extra1a.y = 5.93
extra1a.z = .2
targetsPointCloud.points.append(extra1a)
extra1b = Point32()
extra1b.x = -86.62
extra1b.y = 5.96
extra1b.z = .2
targetsPointCloud.points.append(extra1b)
extra3a = Point32()
extra3a.x = -85.99
extra3a.y = 22.73
extra3a.z = .2
targetsPointCloud.points.append(extra3a)
extra3b = Point32()
extra3b.x = -86.
extra3b.y = 22.73
extra3b.z = .2
targetsPointCloud.points.append(extra3b)
extra4a = Point32()
extra4a.x = -79.92
extra4a.y = 16.66
extra4a.z = .2
targetsPointCloud.points.append(extra4a)
extra4b = Point32()
extra4b.x = -79.93
extra4b.y = 16.64
extra4b.z = .2
targetsPointCloud.points.append(extra4b)
while not rospy.is_shutdown():
pub_pointcloud.publish(targetsPointCloud)
rate.sleep()
def __init__(self):
# Adjustable Parameters
self.path_directory = rospy.get_param("~path_directory")
self.mission_sound = rospy.get_param("~mission_sound")
self.default_sound = rospy.get_param("~default_sound")
self.battery_threshold = int(rospy.get_param("~battery_threshold", 50)) #[%]
# Define Paths' File Name
self.path_dict = ["HOME_A", "HOME_B", "HOME_1", "HOME_2", "HOME_3", "HOME_4", "HOME_5",
"A_1", "A_2", "A_3", "A_4", "A_5", "B_1", "B_2", "B_3", "B_4", "B_5", "1_A",
"2_A", "3_A", "4_A", "5_A", "1_B", "2_B", "3_B", "4_B", "5_B",
"A_HOME", "B_HOME", "1_HOME", "2_HOME", "3_HOME", "4_HOME", "5_HOME", "HOME_CHARGE", "CHARGE_HOME"]
# Define Actions Type
self.action_dict = {0:"None", 1:"Table_Picking", 2:"Table_Dropping"}
# Internal USE Variables - Modify with consultation
self.rate = rospy.Rate(5) # 5 [hz] <--> 0.2 [sec]
self.tf2Buffer = tf2_ros.Buffer()
self.tf2Listener = tf2_ros.TransformListener(self.tf2Buffer)
# - Charging
self.battery_level = 100
self.battery_safe = True
self.battery_full = False
self.go_charge = False
self.start_charge = False
self.go_home = False
# - Point to Point
self.route_list = []
self.route_seq = 0
self.action_list = []
self.action_seq = 0
self.waypoint_list = []
self.waypoint_seq = 0
# - Status for Web
self.delivery_status = ""
self.action_status = True
self.last_msg = None
self.route_once = True
self.XYZ = Point32()
self.location = "HOME"
self.speed = 0
self.delivery_id = "0"
self.delivery_mission = ""
self.mission_activity = "NO ACTION"
# - Others
self.fsm_state = "STANDBY"
self.restore = False
# Publisher
self.all_pub = rospy.Publisher("/web/all_status", String, queue_size=1)
# Subscribers
self.battery_sub = rospy.Subscriber("/battery/percent", String, self.batteryCB, queue_size=1)
self.mission_sub = rospy.Subscriber("/web/mission", String, self.missionCB, queue_size=1)
self.fsm_sub = rospy.Subscriber("/fsm_node/state", FSMState, self.fsmCB, queue_size=1)
self.odom_sub = rospy.Subscriber("/gyro/odom", Odometry, self.odomCB, queue_size=1)
# Service Servers
self.route_done_srv = rospy.Service("/route/done", Empty, self.route_doneSRV)
self.charging_done_srv = rospy.Service("/charging/done", Empty, self.charging_doneSRV)
self.pick_table_done_srv = rospy.Service("/pick_table/done", Empty, self.action_doneSRV)
self.drop_table_done_srv = rospy.Service("/drop_table/done", Empty, self.action_doneSRV)
self.restore_srv = rospy.Service("/restore/call", Empty, self.restoreSRV)
# Service Clients
self.path_call = rospy.ServiceProxy("/change_path", ChangePath)
self.charging_call = rospy.ServiceProxy("charging/call", Empty)
self.pick_table_call = rospy.ServiceProxy("/pick_table/call", Empty)
self.drop_table_call = rospy.ServiceProxy("/drop_table/call", Empty)
self.rosbag_start_call = rospy.ServiceProxy("/rosbag/start", SetFileName)
self.rosbag_stop_call = rospy.ServiceProxy("/rosbag/stop", Empty)
self.fsm_call = rospy.ServiceProxy("/fsm_node/set_state", SetFSMState)
self.sound_call = rospy.ServiceProxy("/sound/call", SetFileLocation)
# Startup
self.sound_call(self.default_sound)
# Main Loop()
self.main_loop()
#!/usr/bin/python
from sensor_msgs.msg import PointCloud
from geometry_msgs.msg import Point32
import rospy
import math
rospy.init_node('clock')
pub = rospy.Publisher('/points', PointCloud)
r = rospy.Rate(1)
while not rospy.is_shutdown():
secs = int(rospy.Time.now().to_sec() % 60)
angle = -secs / 60.0 * 2 * math.pi
pc = PointCloud()
pc.header.stamp = rospy.Time.now()
pc.header.frame_id = '/base_link'
pc.points.append(Point32(math.cos(angle), math.sin(angle), 0))
pub.publish(pc)
r.sleep()
def getXYZ(self, frame_1, frame_2):
try:
transform = self.tf2Buffer.lookup_transform(frame_1, frame_2, rospy.Time())
self.XYZ = Point32(transform.transform.translation.x, transform.transform.translation.y, 0)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
pass
def pub_point(topic, point):
tar_pos = Point32()
tar_pos.x = point[0]
tar_pos.y = point[1]
topic.publish(tar_pos)
def cep_gen_control_radial_force(self, arm, cep, cep_vel):
self.log_state(arm)
if self.started_pulling_on_handle == False:
cep_vel = 0.02
#step_size = 0.01 * cep_vel
step_size = 0.1 * cep_vel # 0.1 is the time interval between calls to the equi_generator function (see pull)
stop = self.common_stopping_conditions()
wrist_force = self.robot.get_wrist_force(arm, base_frame=True)
mag = np.linalg.norm(wrist_force)
curr_pos, _ = self.robot.get_ee_jtt(arm)
if len(self.ee_list) > 1:
start_pos = np.matrix(self.ee_list[0]).T
else:
start_pos = curr_pos
#mechanism kinematics.
if self.started_pulling_on_handle:
self.mech_traj_pub.publish(
Point32(curr_pos[0, 0], curr_pos[1, 0], curr_pos[2, 0]))
self.fit_circle_lock.acquire()
rad = self.rad
cx_start, cy_start = self.cx_start, self.cy_start
cz_start = self.cz_start
self.fit_circle_lock.release()
cx, cy = cx_start, cy_start
cz = cz_start
print 'cx, cy, r:', cx, cy, rad
radial_vec = curr_pos - np.matrix([cx, cy, cz]).T
radial_vec = radial_vec / np.linalg.norm(radial_vec)
if cy_start < start_pos[1, 0]:
tan_x, tan_y = -radial_vec[1, 0], radial_vec[0, 0]
else:
tan_x, tan_y = radial_vec[1, 0], -radial_vec[0, 0]
if tan_x > 0. and (start_pos[0, 0] - curr_pos[0, 0]) < 0.09:
tan_x = -tan_x
tan_y = -tan_y
if cy_start > start_pos[1, 0]:
radial_vec = -radial_vec # axis to the left, want force in
# anti-radial direction.
rv = radial_vec
force_vec = np.matrix([rv[0, 0], rv[1, 0], 0.]).T
tangential_vec = np.matrix([tan_x, tan_y, 0.]).T
tangential_vec_ts = tangential_vec
radial_vec_ts = radial_vec
force_vec_ts = force_vec
if arm == 'right_arm' or arm == 0:
if force_vec_ts[1, 0] < 0.: # only allowing force to the left
force_vec_ts = -force_vec_ts
else:
if force_vec_ts[1, 0] > 0.: # only allowing force to the right
force_vec_ts = -force_vec_ts
f_vec = -1 * np.array(
[wrist_force[0, 0], wrist_force[1, 0], wrist_force[2, 0]])
f_rad_mag = np.dot(f_vec, force_vec.A1)
err = f_rad_mag - 2.
if err > 0.:
kp = -0.1
else:
kp = -0.2
radial_motion_mag = kp * err # radial_motion_mag in cm (depends on eq_motion step size)
radial_motion_vec = force_vec * radial_motion_mag
eq_motion_vec = copy.copy(tangential_vec)
eq_motion_vec += radial_motion_vec
self.prev_force_mag = mag
if self.init_tangent_vector == None or self.started_pulling_on_handle == False:
self.init_tangent_vector = copy.copy(tangential_vec_ts)
c = np.dot(tangential_vec_ts.A1, self.init_tangent_vector.A1)
ang = np.arccos(c)
if np.isnan(ang):
ang = 0.
tangential_vec = tangential_vec / np.linalg.norm(
tangential_vec) # paranoia abot vectors not being unit vectors.
dist_moved = np.dot((curr_pos - start_pos).A1, tangential_vec_ts.A1)
ftan = abs(np.dot(wrist_force.A1, tangential_vec.A1))
self.ft.tangential_force.append(ftan)
self.ft.radial_force.append(f_rad_mag)
if self.ft.type == 'rotary':
self.ft.configuration.append(ang)
else: # drawer
print 'dist_moved:', dist_moved
self.ft.configuration.append(dist_moved)
if self.started_pulling_on_handle:
self.force_traj_pub.publish(self.ft)
# if self.started_pulling_on_handle == False:
# ftan_pull_test = -np.dot(wrist_force.A1, tangential_vec.A1)
# print 'ftan_pull_test:', ftan_pull_test
# if ftan_pull_test > 5.:
# self.started_pulling_on_handle_count += 1
# else:
# self.started_pulling_on_handle_count = 0
# self.init_log() # reset logs until started pulling on the handle.
# self.init_tangent_vector = None
#
# if self.started_pulling_on_handle_count > 1:
# self.started_pulling_on_handle = True
if abs(dist_moved) > 0.09 and self.hooked_location_moved == False:
# change the force threshold once the hook has started pulling.
self.hooked_location_moved = True
self.eq_force_threshold = ut.bound(mag + 30., 20., 80.)
self.ftan_threshold = 1.2 * self.ftan_threshold + 20.
if self.hooked_location_moved:
if abs(tangential_vec_ts[2,
0]) < 0.2 and ftan > self.ftan_threshold:
stop = 'ftan threshold exceed: %f' % ftan
else:
self.ftan_threshold = max(self.ftan_threshold, ftan)
if self.hooked_location_moved and ang > math.radians(90.):
print 'Angle:', math.degrees(ang)
self.open_ang_exceed_count += 1
if self.open_ang_exceed_count > 2:
stop = 'opened mechanism through large angle: %.1f' % (
math.degrees(ang))
else:
self.open_ang_exceed_count = 0
cep_t = cep + eq_motion_vec * step_size
cep_t = cep + np.matrix([-1., 0., 0.]).T * step_size
if cep_t[0, 0] > 0.1:
cep[0, 0] = cep_t[0, 0]
cep[1, 0] = cep_t[1, 0]
cep[2, 0] = cep_t[2, 0]
print 'CEP:', cep.A1
stop = stop + self.stopping_string
return stop, (cep, None)
