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
