def FK_check(self):
self.status_message = "Checking Forward Kinematics"
self.current_state = "FK_check"
kine.FK_dh(self.rexarm.get_positions())
self.rexarm.disable_torque()
self.rexarm.get_feedback()
self.next_state="idle"
def get_wrist_pose(self):
if self.FK == 'pox':
(x, y, z, phi) = kinematics.FK_pox(self)
elif self.FK == 'dh':
(x, y, z, phi) = kinematics.FK_dh(self)
return [-x, y, z, phi]
def record_joints(self):
self.history = np.vstack(
[self.history, np.copy(self.rexarm.joint_angles_fb)])
self.world_history = np.vstack([
self.world_history,
kinematics.FK_dh(np.copy(self.rexarm.joint_angles_fb), 6).t
])
def draw_robot(joints):
Tlist = []
for i in range(len(joints) + 1):
Tlist.append(kinematics.FK_dh(np.array(joints), i).matrix)
olist = []
for T in Tlist:
draw_T(T)
olist.append(list(T2o(T)))
draw_stickbot(olist)
def __init__(self, rexarm):
self.idle = True
self.rexarm = rexarm
self.num_joints = rexarm.num_joints
self.initial_wp = np.array([0.0] * self.num_joints)
self.final_wp = np.array([0.0] * self.num_joints)
self.dt = 0.02 # command rate
self.history = np.copy(self.rexarm.joint_angles_fb)
self.world_history = kinematics.FK_dh(self.history[0], 6).t
self.grip = 0
self.traj_obj = [np.zeros(6), np.zeros(6)]
def execute(self):
self.status_message = "State: Executing movements..."
# Old execute functionality
'''
for pos in self.waypointlist:
self.rexarm.set_speeds_normalized_global(.1)
self.rexarm.set_positions(pos)
self.rexarm.pause(3)
'''
# New functionality:
'''
testlist = [np.array([ 0.0, 0.0, 0.0, 0.0, 0.0]),
np.array([ 1.0, 0.8, 1.0, 0.5, 1.0]),
np.array([-1.0,-0.8,-1.0,-0.5, -1.0]),
np.array([-1.0, 0.8, 1.0, 0.5, 1.0]),
np.array([1.0, -0.8,-1.0,-0.5, -1.0]),
np.array([ 0.0, 0.0, 0.0, 0.0, 0.0])]
'''
testlist = []
T_0 = kinematics.FK_dh(np.array([-1.58824929, 0., 1.39626311, 0., 0.785398, 0.]), 6)
T_1 = kinematics.FK_dh(np.array([ 0,0.27925262,0.59341182,0,0,-0.80285129]),6)
T_2 = kinematics.FK_dh(np.array([-1.34390324, 0.75049142, 1.30899667, 0., 0.29670591, 0.]), 6)
T_3 = kinematics.FK_dh(np.array([-1.36135653, 0.71558484, 1.570796, 0., 0.03490658, 0.]), 6)
T_4 = kinematics.FK_dh(np.array([0, 0, 0, 0, 0, 0]),6)
T_5 = kinematics.FK_dh(np.array([0., 0.97738418, 1.06465062, 0, 0.3141592, -0.89011773]),6)
print("====================================")
print(np.append(T_0.t, T_0.get_euler_angles()))
T_6 = kinematics.FK_dh(np.array([0, 0, 0, 0, 0, 0]),6)
testlist.append(np.append(np.append(T_1.t, T_1.get_euler_angles()), 0))
testlist.append(np.append(np.append(T_2.t, T_2.get_euler_angles()), 0))
testlist.append(np.append(np.append(T_3.t, T_3.get_euler_angles()), 0))
testlist.append(np.append(np.append(T_3.t, T_3.get_euler_angles()), 1))
testlist.append(np.array([1]))
testlist.append(np.append(np.append(T_4.t, T_4.get_euler_angles()), 1))
testlist.append(np.append(np.append(T_5.t, T_5.get_euler_angles()), 1))
testlist.append(np.append(np.append(T_5.t, T_5.get_euler_angles()), 0))
testlist.append(np.append(np.append(T_6.t, T_6.get_euler_angles()), 0))
print("Print out transform.")
# print(np.append(T_1.t, T_1.get_euler_angles()))
# print(np.append(T_2.t, T_2.get_euler_angles()))
'''
Example waypointlist:
[np.array([x,y,z,euler1,2,3,gripper_boolean])]
normal euler configuration: 0.66064978,
-0.46117095, 2.09082787
'''
# print(testlist)
first_position = np.array([146, 0, 45, -np.pi, 0, 0, 1])
offset = np.array([0,45,0,0,0,0,1])
line_em_up_list = [first_position,first_position+offset,first_position+(2*offset),first_position+(3*offset)]
self.tp.execute_plan(line_em_up_list, viz=False, look_ahead=4, controller = 'IK')
self.current_state = "execute"
self.next_state = "idle"
self.rexarm.get_feedback()
def set_positions(self, joint_angles, update_now=True):
joint_angles = np.array(joint_angles)
self.clamp(joint_angles)
T_print = kinematics.FK_dh(joint_angles, 6)
print('current pose: ', np.append(T_print.t,
T_print.get_euler_angles()))
try:
for i, joint in enumerate(self.joints):
self.position[i] = joint_angles[i]
if (update_now):
joint.set_position(joint_angles[i])
except:
return 1
def __init__(self, task):
# predefined values
self.block_height_ = 33
self.identical_threshold_ = 1000 # distance threshold
self.lift_distance_ = 30
self.pick_distance_ = 18
self.push_distance_ = 10
hold_T = kinematics.FK_dh(np.array([0, 0, 0, 0, 0, 0]), 6)
self.hold_position_ = np.append(hold_T.t, hold_T.get_euler_angles())
self.predefined_place_position_ = np.array([186, 0, 0, -np.pi, 0, 0])
# place plan: 0, 1
# 0 : a pre-defined place position
# 1 : a user-defined place position
self.task_ = task
# data storage structure
# position is a 4d location (x, y, z, theta)
self.place_position_ = self.predefined_place_position_
self.place_floor_ = 0 # height of current tower
self.height_map_ = {
1: 33.0,
2: 70.0007701,
3: 105.982281,
4: 152.896345
}
self.predefined_pose = {
4:
np.array([
1.81893597e+02, 3.80334021e+00, 1.66539237e+02, 3.13788307e+00,
-2.09407911e-01, 3.52962487e-02
]),
5:
np.array([
1.71039168e+02, -5.73653219e+00, 1.85103733e+02,
-1.60585479e+00, -3.37168728e-02, -1.38722553e+00
]),
6:
np.array([
161.66916141, 4.95778355, 232.33232539, 0.31385444, -1.4608904,
2.85541216
]),
7:
np.array([
1.60035059e+02, 2.54526534e+00, 2.72297329e+02,
-4.94056433e-03, -1.50092926e+00, -3.12223585e+00
]),
8:
np.array([
157.60025003, 3.33747212, 303.84739213, 0.94417065,
-1.51085512, 2.19926289
])
}
# color part:
self.color_enable_ = False
'''
0 : white
1 : red
2 : green
3 : yellow
4 : blue
5 : purple
6 : light blue
7 : black
'''
self.predefined_color_order = [1, 2, 3, 4, 5, 6, 7]
# push mode:
self.push_mode_ = False
def execute_plan(self,
plan,
look_ahead=0,
viz=False,
controller='indep_joint'):
'''
Iterate through waypoints specified by:
plan=[np.array([j1,j2,j3,j4,j5]),...]
viz allows for plotting of the trajectories using matplotlib
'''
if controller == 'IK':
for waypoint in plan:
if np.shape(waypoint) == (1, ):
# Pause type waypoint
self.set_initial_wp()
self.pause(waypoint[0])
num_points = int(.25 * waypoint[0] / self.dt)
curr_traj_obj = [
np.vstack([self.initial_wp] * num_points),
np.vstack([np.zeros(6)] * num_points)
]
curr_world = np.vstack(
[kinematics.FK_dh(self.initial_wp, 6).t] * num_points)
self.history = np.vstack([self.history, curr_traj_obj[0]])
self.world_history = np.vstack(
[self.world_history, curr_world])
else:
# Movement type waypoint
joint_waypoint = kinematics.IK(
get_T_from_euler_pos(waypoint[3:-1], waypoint[:3]))
if waypoint[-1] == 1:
self.rexarm.close_gripper()
self.grip = 1
if waypoint[-1] == 0:
self.rexarm.open_gripper()
self.grip = 0
self.set_initial_wp()
self.set_final_wp(joint_waypoint)
num_points = 0 #int(.1/self.dt)
#curr_pause_obj = [np.vstack([self.final_wp] * num_points), np.vstack([np.zeros(6)] * num_points)]
curr_traj_obj = self.go(
max_speed=2.5, lookahead=look_ahead) #high speed = 4
#curr_traj_obj[0] = np.vstack([curr_traj_obj[0],curr_pause_obj[0]])
#curr_traj_obj[1] = np.vstack([curr_traj_obj[1],curr_pause_obj[1]])
self.traj_obj[0] = np.vstack(
[self.traj_obj[0], curr_traj_obj[0]])
self.traj_obj[1] = np.vstack(
[self.traj_obj[1], curr_traj_obj[1]])
self.stop()
if controller == 'indep_joint':
print('trajectory_planner: using independent joint control')
traj_list = 0
v_list = 0
for waypoint in plan:
if np.shape(waypoint) == (1, ):
# Pause type waypoint
self.set_initial_wp()
self.pause(waypoint[0])
curr_traj = self.generate_cubic_spline(
self.initial_wp, self.initial_wp, waypoint[0])[0]
else:
# Movement type waypoint
self.set_initial_wp()
self.set_final_wp(waypoint)
curr_traj_obj = self.go(max_speed=1, lookahead=look_ahead)
curr_traj = curr_traj_obj[0]
curr_v = curr_traj_obj[1]
# Add waypoint to list for plotting
if type(traj_list) == int:
traj_list = curr_traj
v_list = curr_v
else:
traj_list = np.vstack([traj_list, curr_traj])
v_list = np.vstack([v_list, curr_v])
self.stop()
if viz:
fig = plt.figure()
ax1 = plt.subplot(121)
for i in range(5):
ax1.plot(np.transpose(traj_list)[i],
label='Joint {} command'.format(i + 1),
color=COLORS[i],
linestyle='--')
ax1.plot(np.transpose(self.history)[i],
label='Joint {} trajectory'.format(i + 1),
color=COLORS[i],
linestyle='-')
ax1.legend(loc='best')
# Velocity plotting
ax2 = plt.subplot(122)
for i in range(5):
ax2.plot(np.transpose(v_list)[i],
label='Joint {} command'.format(i + 1),
color=COLORS[i],
linestyle='--')
#ax2.plot(np.transpose(self.history)[i], label='Joint {} trajectory'.format(i + 1), color=COLORS[i], linestyle='-')
ax2.legend(loc='best')
plt.tight_layout()
plt.show()