def on_message(self, message):
for client in clients:
client.write_message(message)
global timestamps
timestamps.append(time.time())
self.k += 1
data = json.loads(message)
left = data["left"]
right = data["right"]
points_left = [left["shoulder"], left["elbow"], left["wrist"]]
angles_left = kinematics.inverse_kinematics(points_left, "left")
points_right = [right["shoulder"], right["elbow"], right["wrist"]]
angles_right = kinematics.inverse_kinematics(points_right, "right")
left = angles_left
right = angles_right
# print(left)
# print(right)
# sys.exit()
# print(len(angles["left"][0].filtered))
# print(right[0])
for i, item in enumerate(right):
angles["right"][i].push(right[i])
angles["left"][i].push(left[i])
if len(angles["right"][i].filtered) > 0:
pub_right[i].publish(angles["right"][i].filtered[-1])
pub_left[i].publish(angles["left"][i].filtered[-1])
# print("{} ".format(angles["right"][i].filtered[-1] *180 / math.pi))
pass
def twist_callback(self, data):
#We can't do zero point turning, so velocity must be nonzero
if data.linear.x == 0 and data.angular.z != 0:
data.linear.x = 0.00001
#Get initial wheel angular velocities from IK
temp_left, temp_right = kinematics.inverse_kinematics(
data.linear.x,
data.angular.z,
track=self.track,
diameter=self.wheel_diameter)
#Don't bother doing all this if we're stopped
if temp_right != 0 or temp_left != 0:
#Factor used for determining minimum turn radius
radius_limiter = (2 * self.min_turn_radius - self.track) / (
2 * self.min_turn_radius + self.track)
speed_factor = temp_right + temp_left
#Check if radius is too tight. If so, increase speed of inside wheel to match min turn radius
if data.linear.x > 0 and data.angular.z > 0 and temp_left < radius_limiter * temp_right:
temp_left = radius_limiter * temp_right
elif data.linear.x < 0 and data.angular.z > 0 and temp_right > radius_limiter * temp_left:
temp_right = radius_limiter * temp_left
elif data.linear.x > 0 and data.angular.z < 0 and temp_right < radius_limiter * temp_left:
temp_right = radius_limiter * temp_left
elif data.linear.x < 0 and data.angular.z < 0 and temp_left > radius_limiter * temp_right:
temp_left = radius_limiter * temp_right
#Scale velocities back down so linear velocity is the same
new_speed_factor = temp_left + temp_right
temp_right *= speed_factor / new_speed_factor
temp_right *= speed_factor / new_speed_factor
#Check if motor limits are exceeded. If so, clip to limit and reduce other motor proportionally
if temp_left > self.motor_max:
temp_right *= self.motor_max / temp_left
temp_left = self.motor_max
elif temp_left < self.motor_min:
temp_right *= self.motor_min / temp_left
temp_left = self.motor_min
if temp_right > self.motor_max:
temp_left *= self.motor_max / temp_right
temp_right = self.motor_max
elif temp_right < self.motor_min:
temp_left *= self.motor_min / temp_right
temp_right = self.motor_min
#Set final angular velocities
#self.left = temp_left
#self.right = temp_right
#angular velocities to encoder ticks
# self.left = temp_left / (2 * 3.14) * 12 * 81
# self.right = temp_right / (2 * 3.14) * 12 * 81
self.left = temp_left * self.encoder_ticks_per_rad
self.right = temp_right * self.encoder_ticks_per_rad
def inverse_kinematics(self, tool0_frame_RCS):
"""Inverse kinematics function.
Args:
tool0_frame_RCS (Frame): The tool0 frame to reach in robot
coordinate system (RCS).
Returns:
list: A list of possible configurations.
"""
return inverse_kinematics(tool0_frame_RCS, self.params)
def twist_callback_no_limits(self, data):
#Get initial wheel angular velocities from IK
temp_left, temp_right = kinematics.inverse_kinematics(
data.linear.x,
data.angular.z,
track=self.track,
diameter=self.wheel_diameter)
#angular velocities to encoder ticks
self.left = temp_left * self.encoder_ticks_per_rad
self.right = temp_right * self.encoder_ticks_per_rad
rospy.loginfo(destination)
for s,d in zip(pw,destination):
# rospy.loginfo(make_trajectory(s,d))
pos_list.extend(make_trajectory(s,d))
rospy.loginfo(pos_list)
else:
pos_list = []
# rospy.loginfo(estimator.position_camera)
# pos_list = [point1[:3]+np.array([0,0,0.05]),point1[:3],point1[:3]+np.array([0,0,0.05]),point2[:3]+np.array([0,0,0.05]),point2[:3]+np.array([0,0,0.02]),point2[:3]+np.array([0,0,0.05])]
grip = [False,True,True,True,False,False] * len(pw)
yaw = [0]*12 + [0]*3 + [np.pi/2]*3 + [0]*3 + [np.pi/2]*3
rospy.loginfo(pos_list)
for pos,y in zip(pos_list,yaw):
q = kin.inverse_kinematics(pos,y)
print(kin.forward_kinematics(q)[0]["joint_4"])
if q!=None:
target_joints.append(q)
else:
print("No solution")
exit()
arm_controller.home_arm()
arm_controller.open()
for joint,g in zip(target_joints,grip):
arm_controller.set_joint_state(joint)
while(not arm_controller.has_converged()):
pass
if g == True:
arm_controller.close()
#forward forward_kinematics
T = forward_kinematics(
np.array([
deg_to_rad(45),
deg_to_rad(-45),
deg_to_rad(-45),
deg_to_rad(45),
deg_to_rad(0),
deg_to_rad(-45)
]), r6Robot)
print("T = ")
print(T)
print()
#inverse_kinematics
q = inverse_kinematics(T)
print("joints")
print([i / math.pi * 180 for i in q])
print()
#check
T1 = forward_kinematics(np.array(q), r6Robot)
print("T1 = ")
print(T1)
print()
pi = math.pi
angles = [-pi / 3, pi / 6, pi / 2]
R = eulerAnglesXYXToRotationMatrix(angles)
print("euler angles = ")
print(angles)
pass
tilt_controller.set_cam_state(np.deg2rad(-38))
while (not tilt_controller.has_converged()):
pass
rospy.sleep(1)
print("DONE")
H_c2w = kin.cam_to_world(estimator.pan, estimator.tilt)
if len(estimator.p) == 1:
pw = [np.dot(H_c2w, p)[:3] for p in estimator.p]
else:
print("no block detected")
# rospy.loginfo(estimator.position_camera)
# pos_list = [point1[:3]+np.array([0,0,0.05]),point1[:3],point1[:3]+np.array([0,0,0.05]),point2[:3]+np.array([0,0,0.05]),point2[:3]+np.array([0,0,0.02]),point2[:3]+np.array([0,0,0.05])]
arm_controller.home_arm()
for pos in pw:
q = kin.inverse_kinematics(pos, 0) #ADD DESIRED YAW
if q != None:
arm_controller.set_joint_state(q)
while (not arm_controller.has_converged()):
pass
arm_controller.open()
print("reached set point")
rospy.sleep(1)
print("servoing in xy now")
(x, y) = serv.servo_xy(arm_controller, servo)
print("done servoing xy")
print("servoing in z now")
rospy.sleep(1)
serv.servo_z(arm_controller, servo)
arm_controller.close()
serv.servo_z(arm_controller, servo, 'up')
# rospy.sleep(2)
# target_joints = [[0,0,0,0,0],[0,10,20,0,0]]
target_joints = []
pos_list = [[ 0.3, 0.1, 0]]
# pos_list = [[]]
# for tilt in target_tilt:
# tilt_controller.set_cam_state(tilt)
# while(not tilt_controller.has_converged()):
# pass
# for pan in target_pan:
# pan_controller.set_cam_state(pan)
# while(not pan_controller.has_converged()):
# pass
for pos in pos_list:
q = kin.inverse_kinematics(pos,np.deg2rad(45))
if q!=None:
target_joints.append(q)
else:
print("No solution")
exit()
rospy.sleep(2)
# target_joints = [[np.deg2rad(20),0,0,0,0]]
# print(target_joints)
arm_controller.home_arm()
arm_controller.open()
for joint in target_joints:
arm_controller.set_joint_state(joint)
while(not arm_controller.has_converged()):
pass
