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))