def main(): rospy.init_node('Demo') pub = rospy.Publisher('/ur5_model/joint_command', JointAngles, queue_size=10) start_pose = JointAngles([0, 1.8, 5.82, 0, 0, 0]) IK = InverseKinematics() P7 = PathPolynomial7() PC = PathCartesian() dt = 0.05 # Let ROS node start up rospy.sleep(1.0) radius = 0.5 scalar = 1.0 counter = 0 extension = 0.25 tf1 = 1.5 tf2 = 0.35 while not rospy.is_shutdown(): x = radius * np.sin(counter / scalar) y = radius * np.cos(counter / scalar) z = 0.1 phi = np.arctan2(y, x) theta = -4*np.pi/5 ep1 = EffectorPose(x, y, z, phi - np.pi / 2, theta, 0) ep2 = EffectorPose(np.cos(phi) * abs(np.sin(theta)) * extension + x, np.sin(phi) * abs(np.sin(theta)) * extension + y, np.cos(theta) * extension + z, phi, theta, 0) pose = IK.calculate_closest_permutation(ep1, start_pose) out_pose = P7.calculate_and_publish_end_stop_path(start_pose, pose, dt, tf1) rospy.sleep(0.5) out_pose = PC.calculate_and_publish_path(out_pose, ep1, ep2, dt, tf2) rospy.sleep(0.25) PC.calculate_and_publish_path(out_pose, ep2, ep1, dt, tf2) rospy.sleep(0.5) P7.calculate_and_publish_end_stop_path(pose, start_pose, dt, tf1) rospy.sleep(0.5) counter += 1
hand_sub = rospy.Subscriber( '/iros_vision_node/hand', ObjectCoords, lambda msg: keys.hand.update( msg.x, msg.y, msg.width, msg.height, msg.area, msg.angle)) play_sound_pub = rospy.Publisher('/play_sound_file', String, queue_size=1) music_path = os.path.join(rospkg.get_pkg_dir('events'), 'static') # Initializing modules control_module = ControlModule('iros2019', robot_size) # ik config ik_config = os.path.join(rospkg.get_pkg_dir('events'), 'config/iros2019/piano_values.json') inv_kin = InverseKinematics() # Enabling walking module sleep(10) calibrate_robot_pos() frame = np.zeros((50, 50, 3), np.uint8) if with_ik: cal = raw_input('Calibrate? (y/n)') while cal == 'y': calculate_ik_values() sleep(1) control_module.action().play_action("key_ready") sleep(1) cal = raw_input('Calibrate again? (y/n)')
def __init__(self): self.FK = UR5ForwardKinematics() self.IK = InverseKinematics() self.P7 = PathPolynomial7() self.joint_pub = rospy.Publisher("/ur5_model/joint_command", JointAngles, queue_size=10)
from effector_pose import EffectorPose from forward_kinematics import UR5ForwardKinematics from inverse_kinematics import InverseKinematics FK = UR5ForwardKinematics() IK = InverseKinematics() ep = EffectorPose(0.4, 0, 0, 0.75, -1.1415, -0.111111) joints = IK.calculate_joints(ep) angles = FK.effectorEulerAngles(joints[0]) print('Phi') print(angles[0]) print('Theta') print(angles[1]) print('Psi') print(angles[2])
class PathCartesian: """ Makes a path from pose to pose that uses a 7-degree polynomial path """ def __init__(self): self.FK = UR5ForwardKinematics() self.IK = InverseKinematics() self.P7 = PathPolynomial7() self.joint_pub = rospy.Publisher("/ur5_model/joint_command", JointAngles, queue_size=10) def calculate_and_publish_path(self, start_pose, effector_pose1, effector_pose2, dt, tf): """ Publishes the path and returns the last pose JointAngles pose """ t = np.arange(0, tf, dt) XYZ = self.calculate_xyz_path(effector_pose1, effector_pose2, t, tf) arm_angle_path = self.calculate_angle_time_series(start_pose, XYZ) self.publish_arm_path(arm_angle_path, dt, tf) return arm_angle_path[-1] def publish_arm_path(self, ang, dt, tf): """ Given angles and a time step, will try to publish the arm positions in time This function is slightly different than the one in Polynomial Path 7 """ counter = 0.0 for i in range(len(ang)): self.joint_pub.publish(ang[i]) # rospy.loginfo('Publishing, time elapsed = %.2f / %.2f', counter * dt, tf) rospy.sleep(dt) counter += 1 def calculate_xyz_path(self, effector_pose1, effector_pose2, t, tf): """ Takes in pose1 and pose2 and generates a 7-polynomial xyz path with zero initial v, a, j """ xyz1 = [effector_pose1.px, effector_pose1.py, effector_pose1.pz] xyz2 = [effector_pose2.px, effector_pose2.py, effector_pose2.pz] if abs(xyz1[0] - xyz2[0]) > self.FK.ZERO_THRESH: x_cf = self.P7.calculate_coefficients(xyz1[0], 0, 0, 0, xyz2[0], 0, 0, 0, tf) X = self.P7.angle_equation(x_cf, t) Y = np.array([xyz1[1] + (xyz2[1] - xyz1[1]) / (xyz2[0] - xyz1[0]) * (x - xyz1[0]) for x in X]) Z = np.array([xyz1[2] + (xyz2[2] - xyz1[2]) / (xyz2[0] - xyz1[0]) * (x - xyz1[0]) for x in X]) elif abs(xyz1[1] - xyz2[1]) > self.FK.ZERO_THRESH: y_cf = self.P7.calculate_coefficients(xyz1[1], 0, 0, 0, xyz2[1], 0, 0, 0, tf) Y = self.P7.angle_equation(y_cf, t) X = np.array([xyz1[0] + (xyz2[0] - xyz1[0]) / (xyz2[1] - xyz1[1]) * (y - xyz1[1]) for y in Y]) Z = np.array([xyz1[2] + (xyz2[2] - xyz1[2]) / (xyz2[1] - xyz1[1]) * (y - xyz1[1]) for y in Y]) elif abs(xyz1[2] - xyz2[2]) > self.FK.ZERO_THRESH: z_cf = self.P7.calculate_coefficients(xyz1[2], 0, 0, 0, xyz2[2], 0, 0, 0, tf) Z = self.P7.angle_equation(z_cf, t) X = np.array([xyz1[0] + (xyz2[0] - xyz1[0]) / (xyz2[2] - xyz1[2]) * (z - xyz1[2]) for z in Z]) Y = np.array([xyz1[1] + (xyz2[1] - xyz1[1]) / (xyz2[2] - xyz1[2]) * (z - xyz1[2]) for z in Z]) else: return [xyz1, xyz2] return [X, Y, Z] def calculate_angle_time_series(self, start_pose, XYZ): """ Takes a start_pose and an XYZ path, then calculates IK along that path """ euler_init = self.FK.effectorEulerAngles(start_pose) ep_init = EffectorPose(XYZ[0][0], XYZ[1][0], XYZ[2][0], euler_init[0][0], euler_init[1][0], euler_init[2][0]) first_pose_step = self.IK.calculate_closest_permutation(ep_init, start_pose) joint_angles = range(len(XYZ[0])) for i in range(len(XYZ[0])): if i == 0: joint_angles[i] = first_pose_step else: joint_angles[i] = self.IK.calculate_closest_permutation( EffectorPose(XYZ[0][i], XYZ[1][i], XYZ[2][i], euler_init[0][0], euler_init[1][0], euler_init[2][0]), joint_angles[i - 1], ) return joint_angles