def milestone2_test():
"""
Milestone 2 : Test
The robot manipulator traectory generation for pick and place task
CoppeliaSim : Scene8_gripper_csv
"""
Tse_initial = [[1, 0, 0, 0.1662], [0, 1, 0, 0], [0, 0, 1, 0.5],
[0, 0, 0, 1]]
Tsc_initial = [[1, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0.025], [0, 0, 0, 1]]
Tsc_goal = [[0, 1, 0, 0], [-1, 0, 0, -1], [0, 0, 1, 0.025], [0, 0, 0, 1]]
# Generating stadoff and gripping transforms
theta = [0, 3 * math.pi / 4, 0]
R_so3 = mr.VecToso3(theta)
R = mr.MatrixExp3(R_so3)
position = [-0.01, 0, 0.20]
Tce_standoff = mr.RpToTrans(R, position)
position = [-0.01, 0, 0.01]
Tce_gripping = mr.RpToTrans(R, position)
trajectory = trajectory_planner.TrajectoryGenerator(
Tse_initial, Tsc_initial, Tsc_goal, Tce_gripping, Tce_standoff, 0.01,
10)
csv_writer.writeDataList(trajectory, 'milestone2_test')
def matrixTrans(S, theta, Tmat):
I = [[1, 0, 0], [0, 1, 0], [0, 0, 1]]
w = np.array([S[0], S[1], S[2]])
v = np.array([S[3], S[4], S[5]])
#print w,v
skewW = ro.VecToso3(w)
skewV = ro.VecToso3(v)
#calculate terms of rodrigues form for exponential rotation matrix
term1 = np.multiply(I, theta)
term2 = np.multiply(1 - cos(theta), skewW)
term3l = theta - sin(theta)
term3r = np.matmul(skewW, skewW)
term3 = np.multiply(term3l, term3r)
R = term1 + term2 + term3 #Rotation matrix of exponential matrix
#print R
P = np.matmul(R, v) #column vector of exponential matrix
#print P
rT, pT = ro.TransToRp(Tmat) # R and P of given matrix T
#perform matrix transformation of form T_prime = ExpMatrix x T
rT_prime = np.matmul(R, rT)
pT_prime = np.matmul(R, pT) + P
return ro.RpToTrans(rT_prime,
pT_prime) #return transformation matrix of T prime
def __init__(self, limb, tf_listener):
self._limb = limb
self._tf_listener = tf_listener
# h: hand, c: camera
try:
self._tf_listener.waitForTransform(
'/' + self._limb + '_hand', '/' + self._limb + '_hand_camera',
rospy.Time(), rospy.Duration(4.0))
(self._t_hc, self._R_hc) = self._tf_listener.lookupTransform(
'/' + self._limb + '_hand', '/' + self._limb + '_hand_camera',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print 'Error! Cannot find [{}_hand_camera] to [{}_hand] tf.'.format(
self._limb, self._limb)
sys.exit(0)
self._R_hc = quaternion_matrix(self._R_hc)
# frame transforms from camera to hand, only look up once
self._T_hc = modern_robotics.RpToTrans(self._R_hc[:3, :3], self._t_hc)
self._Ad_hc = modern_robotics.Adjoint(self._T_hc)
# frame transforms from hand to body, look up in every loop
self._T_bh = np.zeros((4, 4))
self._Ad_bh = np.zeros((6, 6))
self._arm = baxter_interface.limb.Limb(self._limb)
self._kin = baxter_kinematics(self._limb)
def youBot_Tbase(theta, x, y):
"""Computes the transform of youBot base given a base configuration
:param configuration: A 3-vector representing the current configuration of the robot base.
3 variables for the chassis configuration (theta, x, y),
:return: Transform of robot base in space frame
"""
rotation = [0, 0, theta]
position = [x, y, 0.0963]
R_so3 = mr.VecToso3(rotation)
R = mr.MatrixExp3(R_so3)
Tbase = mr.RpToTrans(R, position)
return Tbase
def poseToTransform(pose):
"""Converts a 3D pose (Wx, Wy, Wz, x, y, z) to a homogeneous transformation matrix
:param pose: 2D pose [Wx, Wy, Wz, x, y, z]
Wx, Wy, Wz : Rotation angles
x, y, z : Position
:return: Homogeneous Transformation matrix corresponding to given pose
"""
[Wx, Wy, Wz, x, y, z] = pose
rotation = [Wx, Wy, Wz]
position = [x, y, z]
R_so3 = mr.VecToso3(rotation)
R = mr.MatrixExp3(R_so3)
T = mr.RpToTrans(R, position)
return T
def youBot_Tse(configuration):
"""Computes the transform of youBot End effector given a robot configuration
:param configuration: A 12-vector representing the current configuration of the robot.
3 variables for the chassis configuration (theta, x, y),
5 variables for the arm configuration (J1, J2, J3, J4, J5),
and 4 variables for the wheel angles (W1, W2, W3, W4)
:return: Transform of End effector in space frame
"""
theta, x, y = configuration[0:3]
rotation = [0, 0, theta]
position = [x, y, 0.0963]
R_so3 = mr.VecToso3(rotation)
R = mr.MatrixExp3(R_so3)
Tsb = mr.RpToTrans(R, position)
Tbe = youBot_Tbe(configuration[3:8])
Tse = np.dot(Tsb, Tbe)
return Tse
def get_ik_thetas(S, M, p, init_thetas, alpha=None, beta=None, gamma=None):
R = rot_mat(175.48, -1.59, 0)
Xe = mr.RpToTrans(R, p)
Xs = M
print(T)
#print(M)
#print(S)
eomg = 10**-2
ev = 10**-2
thetalist0 = np.zeros(6)
success = 0
while not success:
[thetalist, success] = mr.IKinSpace(S, M, T, thetalist0, eomg, ev)
thetalist0 = np.random.randn(6) % np.pi
return thetalist % (2 * np.pi), success
def update_hand_to_body_transforms(self):
'''
Update frame transforms for transformation matrix and twist
'''
try:
self._tf_listener.waitForTransform('/base',
'/' + self._limb + '_hand',
rospy.Time(),
rospy.Duration(4.0))
(t,
R) = self._tf_listener.lookupTransform('/base',
'/' + self._limb + '_hand',
rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
print 'Warning! Cannot find [{}_hand] tf to [base].'.format(
self._limb)
R = quaternion_matrix(R)
self._T_bh = modern_robotics.RpToTrans(R[:3, :3], t)
self._Ad_bh = modern_robotics.Adjoint(self._T_bh)
def milestone3_test4():
"""
Milestone 3 : Test4
Check robot controller for generated tragectory (kp = 0, ki = 0)
"""
configuration = [
0.0, 0.0, 0.0, 0.0, -0.35, -0.698, -0.505, 0.0, 0.0, 0.0, 0.0, 0.0
]
Tsc_initial = [[1, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0.025], [0, 0, 0, 1]]
Tsc_goal = [[0, 1, 0, 0], [-1, 0, 0, -1], [0, 0, 1, 0.025], [0, 0, 0, 1]]
# Tsc_goal = [[1, 0, 0, 0.5], [0, 1, 0, 0], [0, 0, 1, 0.025], [0, 0, 0, 1]]
Tse_initial = kinematic_model.youBot_Tse(configuration)
result_configuration = []
result_Xerr = []
gains = [0, 0] # kp, ki
timeStep = 0.01
velocityLimits = [10, 10, 10, 20, 20, 20, 20, 20, 10, 10, 10, 10]
K = 1
# Generating stadoff and gripping transforms
theta = [0, 3 * math.pi / 4, 0]
R_so3 = mr.VecToso3(theta)
R = mr.MatrixExp3(R_so3)
position = [-0.01, 0, 0.20]
Tce_standoff = mr.RpToTrans(R, position)
position = [-0.005, 0, 0.005]
Tce_gripping = mr.RpToTrans(R, position)
# Trajectory generation
print 'Generating trajectory'
trajectory = trajectory_planner.TrajectoryGenerator(
Tse_initial, Tsc_initial, Tsc_goal, Tce_gripping, Tce_standoff,
timeStep, K)
print 'Trajectory generation successfull'
# Control generation
for x in range(len(trajectory) - 1):
Xd = kinematic_model.configurationToTransform(trajectory[x])
Xd_next = kinematic_model.configurationToTransform(trajectory[x + 1])
Tse = kinematic_model.youBot_Tse(configuration)
V, Xerr = controller.FeedbackControl(Tse, Xd, Xd_next, gains, timeStep)
# Calculate Je
Je = kinematic_model.youBot_Je(configuration)
while True:
# Calculating control
cmd = np.dot(np.linalg.pinv(Je), V)
# Control : omega (5 variables), wheel speeds u (4 variables)
control = np.concatenate((cmd[4:9], cmd[0:4]))
next_config = kinematic_simulator.NextState(
configuration, control, timeStep, velocityLimits)
# Joint limit checking
# status, jointVector = kinematic_model.youBot_testJointLimits(next_config[3:8])
# status = True # TODO
status = kinematic_model.youBot_selfCollisionCheck(
next_config[3:8])
jointVector = [0, 0, 0, 0, 0]
if status:
configuration = next_config
break
else:
Je[:, 4:9] = Je[:, 4:9] * jointVector
# Save configuration (Tse) and Xerr per every K iterations
if (x % K) == 0:
# Append with gripper state
gripper_state = trajectory[x][12]
youBot_configuration = np.concatenate(
(configuration, [gripper_state]))
result_configuration.append(youBot_configuration)
result_Xerr.append(Xerr)
completion = round(((x + 1) * 100.0 / len(trajectory)), 2)
sys.stdout.write("\033[F")
print('Generating youBot trajectory controls. Progress ' +
str(completion) + '%\r')
print "Final Error (Xerr) : \n", np.around(Xerr, 5)
configComments = [
"A 12-vector representing the current configuration of the robot",
"\t3 variables for the chassis configuration (theta, x, y)",
"\t5 variables for the arm configuration (J1, J2, J3, J4, J5)",
"\tand 4 variables for the wheel angles (W1, W2, W3, W4)"
]
csv_writer.writeDataList(result_configuration,
name="milestone3_configurations",
comments=configComments)
XerrComments = [
"A 6-vector representing the error twist",
"\t3 variables for the angular velocity error (Wx, Wy, Wz)",
"\t3 variables for the linear velocity error (Vx, Vy, Vz)"
]
csv_writer.writeDataList(result_Xerr,
name="milestone3_Xerr",
comments=XerrComments)
# Displaying Error Plots
labels = ['Wx', 'Wy', 'Wz', 'Vx', 'Vy', 'Vz']
for x in range(6):
plt.plot(np.array(result_Xerr)[:, x], label=labels[x])
plt.ylabel('Xerr')
plt.legend()
plt.show()
def main():
clientID = startSimulation()
# Handles
joint_handles = [-1, -1, -1, -1, -1, -1]
for i in range(0, 6):
joint_handles[i] = sim.simxGetObjectHandle(clientID,
'UR3_joint' + str(i + 1),
sim.simx_opmode_blocking)[1]
base_handle = sim.simxGetObjectHandle(clientID, "UR3",
sim.simx_opmode_blocking)[1]
end_effector_handle = sim.simxGetObjectHandle(clientID,
"UR3_link7_visible",
sim.simx_opmode_blocking)[1]
end_effector_wrt_base = sim.simxGetObjectPosition(
clientID, end_effector_handle, base_handle,
sim.simx_opmode_streaming)[1]
end_effector_quat_wrt_base = sim.simxGetObjectQuaternion(
clientID, end_effector_handle, base_handle,
sim.simx_opmode_streaming)[1]
time.sleep(0.5)
# Get end effector position and rotation wrt base
end_effector_wrt_base = sim.simxGetObjectPosition(
clientID, end_effector_handle, base_handle,
sim.simx_opmode_oneshot_wait)[1]
end_effector_quat_wrt_base = sim.simxGetObjectQuaternion(
clientID, end_effector_handle, base_handle,
sim.simx_opmode_oneshot_wait)[1]
R_end_effector_wrt_base = Rotation.from_quat(
end_effector_quat_wrt_base).as_matrix()
P_end_effector_wrt_base = end_effector_wrt_base
M = mr.RpToTrans(R_end_effector_wrt_base, P_end_effector_wrt_base)
print(M)
# Forward Kinematics
Slist = getSlist(clientID, joint_handles, base_handle)
theta = [
degToRad(180),
degToRad(0),
degToRad(0),
degToRad(0),
degToRad(0),
degToRad(0)
]
T_endeff_in_base = mr.FKinSpace(M, Slist.T, theta)
print(T_endeff_in_base)
success = False
while (not success):
theta_new, success = mr.IKinSpace(Slist.T, M, T_endeff_in_base, [
random.random(),
random.random(),
random.random(),
random.random(),
random.random(),
random.random()
], 0.01, 0.01)
time.sleep(1)
move.setTargetPosition(clientID, joint_handles, theta_new)
errorCode = 1
while (errorCode):
errorCode, end_effector_wrt_base = sim.simxGetObjectPosition(
clientID, end_effector_handle, base_handle,
sim.simx_opmode_oneshot_wait)
print(end_effector_wrt_base)
time.sleep(1)
move.setTargetPosition(clientID, joint_handles, [0, 0, 0, 0, 0, 0])
endSimulation(clientID)
return 1
[0, 0, 0, 0]]
shat = [1, 0, 0]
p = [0, 0, 2]
h = 1
pb = np.array([1, 2, 3]).T
vs = np.array([3, 2, 1, -1, -2, -3]).T
stheta = np.array([0, 1, 2, 3, 0, 0]).T
fb = np.array([1, 0, 0, 2, 1, 0]).T
v = np.array([1, 0, 0, 0, 2, 3]).T
print("Ex1, Tsa:")
Tsa = mr.RpToTrans(Rsa, pVa)
print(Tsa)
# [[0,-1,0,0],[0,0,-1,0],[1,0,0,1],[0,0,0,1]]
print("Ex2, Inverse of Tsb:")
Tsb = mr.RpToTrans(Rsb, pVb)
Tbs = mr.TransInv(Tsb)
print(Tbs)
# [[1,0,0,0],[0,0,-1,0],[0,1,0,-2],[0,0,0,1]]
print("Ex3, Tab:")
Tas = mr.TransInv(Tsa)
Tab = np.dot(Tas, Tsb)
print(Tab)
# [[0,-1,0,-1],[-1,0,0,0],[0,0,-1,-2],[0,0,0,1]]
Blist = np.array(
[[0, 0, 0],
[0, 0, 0],
[1, 1, 1],
[2 * homeydist, homeydist, homeydist],
[0, -homexdist, homexdist],
[0, 0, 0]]
)
thetalist0 = np.array([np.pi/4, np.pi/8, -np.pi/4])
thetalist = []
for i in range(len(waypoints)):
p = waypoints[i]
Tsb = mr.RpToTrans(R, p)
[th, success] = mr.IKinBody(Blist, M, Tsb, thetalist0, eomg, ev)
thetalist.append(th)
f = open("joint_angles.txt", "w")
for i in range(len(thetalist)):
angle = thetalist[i]
f.write(str(angle)+"\n")
f.close()
# ============= Search ============== #
def find_available(node):
available = []
if (node[0] + 0 and node[1]+50) not in available:
def moveBaseXYPhi(self,
endBaseConfig,
pid,
velocity_factor=0.1,
time_scaling_method=MOVEMENT_TIME_SCALING_METHOD_QUINTIC,
dryrun=True):
'''
Given a target body config vector (endBaseConfig : X, Y, phi), move the bot from the current position
to this new position
endBaseConfig = x, y, phi
type : float
velocity_factor = 1 = 100%, 0 = 0%
type : float
dryrun = true : will not drive the robot. False will
'''
# Adjust movement speed
self.current_omega = self.max_omega * velocity_factor
self.current_V = self.max_V * velocity_factor
# Calculate coordinates in space frame (fixed frame starting at robot init)
target_x, target_y, target_phi = endBaseConfig
R = np.array([[1, 0, 0],
[0, 1, 0],
[0, 0, 1]])
p = np.array((target_x, target_y, 0))
Tdest = mr.RpToTrans(R, p)
# Convert rotation + translation from base frame in original space frame
Ts_dest = self.Tsb@Tdest
# use coordinates in space frame to perform move.
R, p = mr.TransToRp(Ts_dest)
target_x, target_y = p[0], p[1]
# calculate spin angle (check for singularity (if theta = -180 or pi or -pi), or if
# there's only a spin angle, no movement in X or Y)
# arctan is defined between -pi/2 and pi/2... so target_x has to be larger
if target_x == self.currentX and target_y == self.currentY:
phi = 0
elif target_x >= self.currentX: # quadrants 1 and 4ar
phi = arctan(np.float64(target_y - self.currentY) / np.float64(target_x - self.currentX))
if np.isnan(phi):
phi = 0
elif target_y >= self.currentY: # arccos is defined between 0 and pi (quadrant 2)
phi = arccos(np.float64(target_x - self.currentX) / (np.sqrt(np.power(target_x - self.currentX, 2) + np.power(target_y - self.currentY, 2))))
elif target_y < self.currentY and target_x < self.currentX: # below the X axis, 3th quadrant
phi = pi + arctan(np.float64(target_y - self.currentY) / np.float64(target_x - self.currentX))
else:
log.info(LOGGER_BASE_MOVEXYPHI, 'We have a problem... check for singularity configurations')
# subtract current angle
phi -= self.currentPhi
# First Spin
self.rotateBasePhi(phi=phi,
pid=pid,
velocity_factor=velocity_factor,
method=time_scaling_method,
dryrun=dryrun)
# Translate
distance = np.linalg.norm((np.array([target_x, target_y]) - np.array([self.currentX, self.currentY])))
self.moveBaseX(distance=distance,
pid=pid,
velocity_factor=velocity_factor,
method=time_scaling_method,
dryrun=dryrun)
# Last spin for final orientation
self.rotateBasePhi(phi=target_phi,
pid=pid,
velocity_factor=velocity_factor,
method=time_scaling_method,
dryrun=dryrun)
se3mat = mr.VecTose3(V6) T = mr.MatrixExp6(se3mat) R, p = mr.TransToRp(T) so3mat = mr.MatrixLog3(R) omega = mr.so3ToVec(so3mat) # print p # print omega #Q06 F = np.array([[0, 0], [0, 0], [-0.25, 0.25], [0.25, 0.25], [0, 0], [0, 0]]) # Finding Tbe omega = np.array([0, 0, math.pi / 2]) p = np.array([2, 3, 0]) so3mat = mr.VecToso3(omega) R = mr.MatrixExp3(so3mat) Tbe = mr.RpToTrans(R, p) Teb = np.linalg.inv(Tbe) Ad_Teb = mr.Adjoint(Teb) Jbase = np.dot(Ad_Teb, F) print Tbe print Jbase # #Q07 Jb = [0, 0, 1, -3, 0, 0] Jx = [0, 0, 1, 0, -2, 0] # print np.dot(Ad_Teb, Jx) # [0,-1,0,2;1,0,0,3;0,0,1,0;0,0,0,1] # 0, 1, 0, -3, # -1, 0, 0, 2 # 0, 0, 1, 0,
# Program constants timeStep = 0.01 velocityLimits = [10, 10, 10, 20, 20, 20, 20, 20, 10, 10, 10, 10] K = 10 result_configuration = [] result_Xerr = [] # Generating stadoff and gripping transforms theta = [0, 3 * math.pi/4, 0] R_so3 = mr.VecToso3(theta) R = mr.MatrixExp3(R_so3) position = [0.00, 0, 0.20] #[-0.01, 0, 0.20] Tce_standoff = mr.RpToTrans(R, position) position = [0.00, 0, 0.00] #[-0.01, 0, 0.00] Tce_gripping = mr.RpToTrans(R, position) # Displaying initial End Effector configuration error initialRef = kinematic_model.transformToPose(Tse_refInitial) initialConf = kinematic_model.transformToPose(Tse_initial) initialError = initialRef - initialConf print 'Initial End Effector configuration Error (Theta_x, Theta_y, Theta_z, x, y, z):\n', np.around(initialError, 3) ## Trajectory generation print 'Generating trajectory' trajectory = trajectory_planner.TrajectoryGenerator(Tse_refInitial, Tsc_initial, Tsc_goal, Tce_gripping, Tce_standoff, timeStep, K ) print 'Trajectory generation successfull'
def main():
n = rospy.init_node('teleoperation_node')
teleoperation = Teleoperation(n)
# wait for a little bit time for everything to be ready
while not teleoperation.is_master_pose_received:
print("Waiting for master pose message...")
rospy.sleep(0.5)
teleoperation.set_master_home_pose(teleoperation.master_pose_original)
r = rospy.Rate(5) # 50 Hz
cnt = 0
rospy.wait_for_service('move_to_pose')
while not rospy.is_shutdown():
try:
req = MoveToPoseRequest()
#
# q_rot1 = Quaternion(degrees=90, axis=(0.0, 0.0, 1.0))
# q_rot2 = Quaternion(degrees=-90, axis=(1.0, 0.0, 0.0))
#
# r_gripper2tool = np.matrix([[0, 0, -1.0], [0, -1.0, 0], [-1.0, 0, 0]])
# q_gripper2tool = Quaternion(matrix=r_gripper2tool)
# # q_d = q_gripper2tool*teleoperation.q_original
# print(teleoperation.q_original.rotation_matrix)
# q_d = q_rot1*q_rot2*teleoperation.q_original
# #q_d = q_rot1 * q_rot2 * teleoperation.q_original
# #print(teleoperation.q_original.rotation_matrix)
# #print(q_d[0], q_d[1], q_d[2], q_d[3])
# q_home_tool = Quaternion(w=0.707, x=0.0, y =0.707, z=0.0)
# print(teleoperation.master_pose_original)
# T_master_current2home = np.matmul(teleoperation.master_pose_original, mr.TransInv(teleoperation.master_home_pose))
master_R_original, master_p_original = mr.TransToRp(
teleoperation.master_pose_original)
master_R_home, master_p_home = mr.TransToRp(
teleoperation.master_home_pose)
master_p_current2home = master_p_original - master_p_home
master_R_current2home = np.matmul(master_R_original,
mr.RotInv(master_R_home))
# print(master_R_current2home, master_p_current2home)
# R_z_90 = np.zeros((3,3))
# R_z_90[0, 1] = 1.0
# R_z_90[1, 0] = -1.0
# R_z_90[2, 2] = 1.0
#
# master_R_current2home = np.matmul(R_z_90, master_R_current2home)
# master_p_current2home = np.matmul(R_z_90, master_p_current2home)
slave_R_home, slave_p_home = mr.TransToRp(
teleoperation.slave_home_pose)
slave_R_des = np.matmul(master_R_current2home, slave_R_home)
slave_p_des = master_p_current2home * teleoperation.position_scale + slave_p_home
T_slave_des = mr.RpToTrans(slave_R_des, slave_p_des)
#print(T_slave_des)
slave_pose_msg = pose_msg_from_matrix(T_slave_des)
print(slave_pose_msg)
req.pose = slave_pose_msg
# dis = Quaternion.distance(q_d, q_home_tool)
# print(master_R_current2home[2, 2])
# if master_R_current2home[2, 2] < 0.8:
# req.pose.orientation.w = 0.707#q_d[0]
# req.pose.orientation.x = 0.0#q_d[1]
# req.pose.orientation.y = 0.707#q_d[2]
# req.pose.orientation.z = 0.0#q_d[3]
# # # print(dis)
# # # print(q_d)
# #
# #
# # # req.pose.orientation.w = q_d[0]
# # # req.pose.orientation.x = q_d[1]
# # # req.pose.orientation.y = q_d[2]
# # # req.pose.orientation.z = q_d[3]
# #
# p_original = teleoperation.current_position - teleoperation.position_offset
#
# p_d = np.matmul(q_rot1.rotation_matrix, p_original) * teleoperation.position_scale + teleoperation.p_rcm
# p_d[2] -= teleoperation.z_tip_offset
#
# req.pose.position.x = p_d[0]
# req.pose.position.y = p_d[1]
# req.pose.position.z = p_d[2]
#
if teleoperation.gripper_close:
req.jaw_angle = 0
else:
req.jaw_angle = np.pi / 3
req.secs = 0.1
# # print(req)
#
move_to_pose = rospy.ServiceProxy('move_to_pose', MoveToPose)
res = move_to_pose(req)
# if res.success:
# print('Successfully moved to the area above pad')
# else:
# print('Failed to move to the area above pad')
except rospy.ServiceException, e:
print "Service call failed: %s" % e
cnt += 1
print(cnt)
rospy.sleep(0.01)
async def asyncProcessMQTTMessages(self, loopDelay):
"""
This function receives the messages from MQTT to move the robot.
It is implemented in another thread so that the killswitch can kill
the thread without knowing which specific
"""
while True:
try:
await asyncio.sleep(loopDelay)
if self.mqttMoveQueue.empty() is False:
try:
if self.currentVoltage < LOW_VOLTAGE_THRESHOLD:
raise LowVoltageException
except LowVoltageException:
log.warning(LOGGER_DDR_THREADIMPLMQTT,
msg=f'Low voltage threshold '
f'{float(LOW_VOLTAGE_THRESHOLD):.2} '
f'reached. Current voltage = '
f'{self.currentVoltage:.2f} .'
f'Robot will not move, but MQTT '
f'message will attempt to process. '
f'Consider charging the battery, '
f'or find some kind of power supply')
# Remove the first in the list, will pause until there
# is something
currentMQTTMoveMessage = self.mqttMoveQueue.get()
# Decode message received
msgdict = json.loads(
currentMQTTMoveMessage.payload.decode('utf-8'))
# Verify if need to urgently overried current movement
# and queue
if 'override' in msgdict:
if msgdict['override']:
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Overriding all motion - calling '
f'killswitch to empty queue')
# Urgently stop the current movement (and empty
# movement queue - done in killSwitch)
self.killSwitch()
# Default motion configuration
pid = False if 'pid' not in msgdict else msgdict["pid"]
velocity_factor = 0.1 if 'velocity_factor' not \
in msgdict else msgdict["velocity_factor"]
dryrun = False if 'dryrun' not in msgdict else \
msgdict["dryrun"]
inter_wheel_distance = None if 'inter_wheel_distance' not\
in msgdict else msgdict["inter_wheel_distance"]
wheel_radius = None if 'wheel_radius' not in msgdict \
else msgdict["wheel_radius"]
time_scaling_method = MOVEMENT_TIME_SCALING_METHOD_DEFAULT
if currentMQTTMoveMessage.topic == 'bot/killSwitch':
self.killSwitch()
elif currentMQTTMoveMessage.topic == \
'bot/base/reset/position':
log.warning(
LOGGER_DDR_IMPLMOVELOOP,
msg=f'Invoking reset body configuration')
self.base.resetBodyConfiguration()
elif currentMQTTMoveMessage.topic == \
'bot/base/move/xyphi':
self.eventLoop.run_in_executor(
self.eventLoopBaseExecutors,
functools.partial(self.base.moveBaseXYPhi,
endBaseConfig=(msgdict["x"],
msgdict["y"],
msgdict["phi"]),
pid=pid,
velocity_factor=velocity_factor,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/base/move/x':
self.eventLoop.run_in_executor(
self.eventLoopBaseExecutors,
functools.partial(self.base.moveBaseX,
distance=msgdict["distance"],
pid=pid,
velocity_factor=velocity_factor,
wheel_radius=wheel_radius,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/base/turn/phi':
self.eventLoop.run_in_executor(
self.eventLoopBaseExecutors,
functools.partial(
self.base.rotateBasePhi,
phi=msgdict["phi"],
pid=pid,
velocity_factor=velocity_factor,
inter_wheel_distance=inter_wheel_distance,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/base/move/home':
# Calculate where to go from there (not avoiding
# obstacles...)
_, (x, y, _) = mr.TransToRp(
mr.TransInv(self.base.Tsb))
# pi - self.base.currentPhi
phi = - 1 * self.base.currentPhi
self.base.moveBaseXYPhi(
endBaseConfig=(x, y, phi),
pid=pid,
velocity_factor=velocity_factor,
time_scaling_method=time_scaling_method,
dryrun=dryrun)
elif currentMQTTMoveMessage.topic == \
'bot/arm/move/rest':
self.eventLoop.run_in_executor(
self.eventLoopArmExecutors,
functools.partial(
self.arm.moveToAngle,
armConfigVector=self.arm.armRestPosition,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/arm/move/zero':
self.eventLoop.run_in_executor(
self.eventLoopArmExecutors,
functools.partial(
self.arm.moveToAngle,
armConfigVector=self.arm.armZeroPosition,
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/arm/move/theta':
self.eventLoop.run_in_executor(
self.eventLoopArmExecutors,
functools.partial(
self.arm.moveToAngle,
armConfigVector=[msgdict["theta1"],
msgdict["theta2"]],
dryrun=dryrun))
elif currentMQTTMoveMessage.topic == \
'bot/gripper/open':
# Already an async call (non blocking)
self.arm.openEndEffector()
elif currentMQTTMoveMessage.topic == \
'bot/gripper/close':
# Already an async call (non blocking)
self.arm.closeEndEffector()
elif currentMQTTMoveMessage.topic == \
'bot/gripper/position':
# Call function to move this thing
Pco = np.array(msgdict["P"])
angle = msgdict["grip_angle"]
Toe_grip = mr.RpToTrans(
R=np.array([[cos(angle), 0, sin(angle)],
[0, 1, 0],
[-sin(angle), 0, cos(angle)]]),
p=[0, 0, 0])
Rco, _ = mr.TransToRp(Toe_grip) # Rco=camera-object
# Rco, _ = mr.TransToRp(self.Toe_grip)
Tco_desired = mr.RpToTrans(R=Rco, p=Pco)
Tbo_desired = self.Tbc @ Tco_desired
# Initial guess - based on the angle received
thetalist0 = (0, 0, pi / 8, pi / 4) # pi / 4, pi / 2)
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Running inverse kinematics to get '
f'required joint angles.')
thetalist, success = mr.IKinBody(
Blist=self.arm.bList,
M=self.M0e,
T=Tbo_desired,
thetalist0=thetalist0,
eomg=0.02, # 0.08 rad = 5 deg uncerainty
ev=0.025) # 4cm error?
if success:
thetalist = thetalist.round(6)
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'IK Solved with joint thetas '
f'found {thetalist}')
# 0. Open gripper
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Opening or ensuring end effector '
f'is opened.')
self.arm.openEndEffector()
# 1. Move arm to 0 position - launch async.
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving arm to driving'
f'position')
self.arm.moveToAngle(
armConfigVector=self.arm.armDrivePosition,
dryrun=dryrun)
# 2. Move base by X first
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving body by X '
f'{thetalist[0]:.2f}')
self.base.moveBaseX(
distance=thetalist[0],
pid=pid,
velocity_factor=velocity_factor,
dryrun=dryrun)
# 3. Rotate base by Phi
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Rotating base by phi '
f'{thetalist[1]:.2f}')
self.base.rotateBasePhi(
phi=thetalist[1],
pid=pid,
velocity_factor=velocity_factor,
dryrun=dryrun)
# 4a. Move to standoff position
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving arm joints to standoff '
f'position')
# 4. Move other joint in position - need to block
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving arm joints into position '
f'for picking up object j3 = '
f'{degrees(thetalist[2]):.2f}, '
f'j4 = {degrees(thetalist[3]):.2f}')
self.arm.moveToAngle(armConfigVector=thetalist[2:],
dryrun=dryrun)
# 5. Grab object
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Closing end effector')
self.arm.closeEndEffector()
# Rest b/c the previous call is non blocking,
# and blocking causes issues on the wait
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Async Sleep for 1 second...')
await asyncio.sleep(1)
# 6. Set to driving position
log.debug(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Moving arm to driving position.')
self.arm.moveToAngle(
armConfigVector=self.arm.armDrivePosition,
dryrun=dryrun)
log.debug(LOGGER_DDR_IMPLMOVELOOP, f'Done!')
else:
log.warning(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Unable to calculate joint and '
f'wheel angles to achieve the '
f'required position.')
elif currentMQTTMoveMessage.topic == \
'bot/logger':
# Changing the logging level on the fly...
log.setLevel(msgdict['logger'], lvl=msgdict['level'])
elif currentMQTTMoveMessage.topic == \
'bot/logger/multiple':
# Changing the logging level on the fly for multiple loggers at a time
for logger, level in msgdict.items():
log.setLevel(logger, level)
else:
raise NotImplementedError
except NotImplementedError:
log.warning(LOGGER_DDR_IMPLMOVELOOP,
msg=f'MQTT message not implemented.')
except asyncio.futures.CancelledError:
log.warning(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Cancelled the MQTT dequeing task.')
except:
log.error(LOGGER_DDR_IMPLMOVELOOP,
msg=f'Error : {traceback.print_exc()}')
def milestone3_test4_debug():
"""
Milestone 3 : Test4
Check robot controller for generated tragectory (kp = 0, ki = 0)
"""
# configuration = [0, 0, 0, 0, -0.35, -0.698, -0.505, 0, 0, 0, 0, 0]
configuration = [
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
]
Tsc_initial = [[1, 0, 0, 1], [0, 1, 0, 0], [0, 0, 1, 0.025], [0, 0, 0, 1]]
Tsc_goal = [[0, 1, 0, 0], [-1, 0, 0, -1], [0, 0, 1, 0.025], [0, 0, 0, 1]]
Tse_initial = kinematic_model.youBot_Tse(configuration)
result_configuration = []
result_Xerr = []
gains = [0, 0] # kp, ki
timeStep = 0.5
K = 1
# Generating stadoff and gripping transforms
theta = [0, 3 * math.pi / 4, 0]
R_so3 = mr.VecToso3(theta)
R = mr.MatrixExp3(R_so3)
position = [-0.01, 0, 0.20]
Tce_standoff = mr.RpToTrans(R, position)
position = [-0.01, 0, 0.01]
Tce_gripping = mr.RpToTrans(R, position)
# Trajectory generation
# trajectory = trajectory_planner.TrajectoryGenerator(Tse_initial, Tsc_initial, Tsc_goal, Tce_gripping, Tce_standoff, timeStep, K )
Xstart = kinematic_model.youBot_Tse(configuration)
XendConfig = [0.0, 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
Xend = kinematic_model.youBot_Tse(XendConfig)
trajectory = mr.ScrewTrajectory(Xstart, Xend, 3, 6, 3)
csv_writer.writeDataList(trajectory, "milestone3_trajectory")
# Control generation
for x in range(len(trajectory) - 1):
Xd = trajectory[x]
Xd_next = trajectory[x + 1]
# Xd = kinematic_model.configurationToTransform(trajectory[x])
# Xd_next = kinematic_model.configurationToTransform(trajectory[x+1])
Tse = kinematic_model.youBot_Tse(configuration)
print "####### Iteration #######"
print "Tse"
print np.around(Tse, 3).tolist()
print "Xd"
print np.around(Xd, 3).tolist()
print "Xd_next"
print np.around(Xd_next, 3).tolist()
V, Xerr = controller.FeedbackControl(Tse, Xd, Xd_next, gains, timeStep)
print "V"
print np.around(V, 3).tolist()
print "Xerr"
print np.around(Xerr, 3).tolist()
# Calculate Je
Je = kinematic_model.youBot_Je(configuration)
while True:
# Calculating control
cmd = np.dot(np.linalg.pinv(Je), V)
# Control : omega (5 variables), wheel speeds u (4 variables)
control = np.concatenate((cmd[4:9], cmd[0:4]))
print "control"
print np.around(control, 3).tolist()
next_config = kinematic_simulator.NextState(
configuration, control, timeStep)
print "next_config"
print np.around(next_config, 3).tolist()
# Joint limit checking
status, jointVector = kinematic_model.youBot_testJointLimits(
next_config[3:8])
if status:
configuration = next_config
break
else:
Je[:, 4:9] = Je[:, 4:9] * jointVector
# Save configuration (Tse) and Xerr per every K iterations
if (x % K) == 0:
result_configuration.append(configuration)
result_Xerr.append(Xerr)
# print np.around(configuration, 3).tolist()
csv_writer.writeDataList(result_configuration, "milestone3_configurations")
def __init__(self,
robot_config_file,
initRobotConfig,
timeStep,
odometryTimer=0.1,
defaultLogging=30):
"""
Creates the main bot class.
"""
if not os.path.exists(robot_config_file):
raise ValueError(f'Cannot find configuration file '
f'"{robot_config_file}"')
with open(robot_config_file, 'r') as f:
self.robotConfiguration = json.load(f)
# Adjust JSON just read
self.__adjustConfigDict(self.robotConfiguration)
# Exception Queue for inter threads exception communications
self.exceptionQueue = queue.Queue()
self.mqttMoveQueue = queue.Queue()
self.baseMovementLock = asyncio.locks.Lock()
self.timeStep = timeStep
self.powerSupply = PowerSupply()
# region sensor Init
self.sensors = {}
for sensor in self.robotConfiguration["sensors"]:
sensorDict = self.robotConfiguration["sensors"][sensor]
legoPort = LegoPort(sensorDict["input"])
# Applies to both I2C and SDK controled sensors
if legoPort.mode != sensorDict["mode"]:
legoPort.mode = sensorDict["mode"]
# Only applies to sensors controled by the python SDK
if "set_device" in sensorDict:
legoPort.set_device = sensorDict["set_device"]
time.sleep(1)
address = sensorDict["input"]
self.sensors[sensor] = sensorDict["sensorType"](address=address)
# endregion
# base Init
self.base = DiffDriveBase(self.robotConfiguration["base"],
initRobotConfig, odometryTimer)
# arm/endeffector Init
self.arm = Arm(armLinkDefinitions=self.robotConfiguration["arm"])
# Other matrix init
self.M0e = mr.RpToTrans(
R=np.array(
self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["M0e"]["p"]) # noqa F501
self.Toe_grip = mr.RpToTrans(
R=np.array(
self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["arm"]["arm1"]["manipulatorConfiguration"]["Toe_grip"]["p"]) # noqa F501
self.Tbc = mr.RpToTrans(
R=np.array(
self.robotConfiguration["cameraConfiguration"]["Tbc"]["rotationMatrix"]).T, # noqa F501
p=self.robotConfiguration["cameraConfiguration"]["Tbc"]["p"])
self.Tcb = mr.TransInv(self.Tbc)
# Reset all motors at init
self.urgentStop = False
log.info(LOGGER_DDR_MAIN, 'Done main robot init.')