def __init__(self):
self.down = -np.pi/6
self.up = np.pi/6
self.gripper = Gripper()
self.num = 50
self.time = 5
def send6(self, value):
Gripper.set_pos(self, 6, value)
print(6, value)
class Tribot(Gripper):
def __init__(self):
self.down = -np.pi/6
self.up = np.pi/6
self.gripper = Gripper()
self.num = 50
self.time = 5
#************* joint's angle computation ****************
# input end-effector position in world frame: self.end_pos_w
# output joint's angle: self.q_r
def cal_joint_angle(self, x_w, y_w):
d = 0.585
l1 = 0.52
l2 = 0.26
l3 = 0.83
theta_2 = np.arcsin((d - x_w)/l3)
theta_1 = np.arcsin(y_w/(l3*np.cos(theta_2) + l2))
z_w = l3*np.cos(theta_1)*np.cos(theta_2) + l2*np.cos(theta_1) + l1
return theta_1, theta_2
def talker(self):
pub11 = rospy.Publisher('/tribot/joint11_position_controller/command',
Float64, queue_size=10)
pub12 = rospy.Publisher('/tribot/joint12_position_controller/command',
Float64, queue_size=10)
pub21 = rospy.Publisher('/tribot/joint21_position_controller/command',
Float64, queue_size=10)
pub22 = rospy.Publisher('/tribot/joint22_position_controller/command',
Float64, queue_size=10)
pub31 = rospy.Publisher('/tribot/joint31_position_controller/command',
Float64, queue_size=10)
pub32 = rospy.Publisher('/tribot/joint32_position_controller/command',
Float64, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(self.num)
arr1 = self.tra_discrete(0.40, self.down, self.up, self.num)
arr2 = self.tra_discrete(0.60, self.down, self.up, self.num)
print(arr1.shape)
arr1 = np.concatenate((arr1[::-1,:], arr2), axis=0)
print(arr1.shape)
print(arr1)
while not rospy.is_shutdown():
for i in range(len(arr1)):
q11, q12 = self.cal_joint_angle(arr1[i][0], arr1[i][1])
q21, q22 = q11, q12
q31, q32 = q11, q12
#rospy.loginfo(q11)
#rospy.loginfo(q12)
pub11.publish(q11)
pub12.publish(q12)
pub21.publish(q21)
pub22.publish(q22)
pub31.publish(q31)
pub32.publish(q32)
ser1 = self.rad2servo(q11)
ser4 = self.rad2servo(q12)
print(q11, q12, ser1, ser4)
print(self.gripper.read_force())
ID_array = np.array([1,2,3,4,5,6])
pos_array = np.array([ser1, ser1, ser1, ser4, ser4, ser4])
self.gripper.set_pos_batch(ID_array, pos_array, self.time)
time.sleep(0.02)
rate.sleep()
rospy.spin()
def rad2servo(self, rad):
return int(-rad * 2000/np.pi + 1500)
def tra_discrete(self, r, down=-np.pi, up=np.pi, num = 10, index = 0):
arr = np.zeros((num, 2))
band = up - down
for i in range(num):
x = r * np.cos(band*i/num + down)
y = r * np.sin(band*i/num + down)
arr[i] = x, y
return arr
def send5(self, value):
Gripper.set_pos(self, 5, value)
print(5, value)
def send4(self, value):
Gripper.set_pos(self, 4, value)
print(4, value)
def send3(self, value):
Gripper.set_pos(self, 3, value)
print(3, value)
def send2(self, value):
Gripper.set_pos(self, 2, value)
print(2, value)
def send1(self, value):
Gripper.set_pos(self, 1, value)
print(1, value)