def __init__(self, server_name):
self.client = actionlib.SimpleActionClient(server_name, FollowJointTrajectoryAction)
self.joint_cmd_pub = rospy.Publisher("/gripper/command", Float64)
self.joint_positions = []
self.names = ["joint1",
"joint2",
"joint3",
"joint4"]
blocks = get_blocks()
block = blocks[0]
# the list of xyz points we want to plan
xy_positions = [
[
block[2][0] + (block[0][0] - block[2][0]) / 2,
block[3][1] + (block[1][1] - block[3][1]) / 2
]
]
# findAngle q4
t1 = findAngle(block[0], block[1])
t2 = findAngle(block[1], block[2])
theta = t1 if t1 < t2 else t2
# Transformation
xyz_newpositions = []
for pos in xy_positions:
xyz_newpositions.append(transformation(pos[0], pos[1]))
# initial duration
dur = rospy.Duration(1)
# construct a list of joint positions by calling invkin for each xyz point
for p in xyz_newpositions:
joint_positions = invkin(p, theta)
jtp = JointTrajectoryPoint(positions=joint_positions, velocities=[0.5] * self.N_JOINTS, time_from_start=dur)
dur += rospy.Duration(2)
self.joint_positions.append(jtp)
self.jt = JointTrajectory(joint_names=self.names, points=self.joint_positions)
self.goal = FollowJointTrajectoryGoal(trajectory=self.jt, goal_time_tolerance=dur + rospy.Duration(2))
