def yaml_wobj(self):
"""
The coordinate system of the paper pattern.
"""
if not self.__yaml_data:
return None
return hom(self.yaml_wobj_ori, self.yaml_wobj_pos)
def main():
j1 = 0
j2 = 0
j3 = 0
j4 = 0
j5 = 0
j6 = 0
joint_values = [j1, j2, j3, j4, j5, j6]
tool = hom(0, 0, 0, [0.1, 0, 0])
dh_table['tool'] = tool
info = forward_kinematics(*joint_values, **dh_table)
# multiplication from left: rotation in base to base
# multiplication from right: rotation in tool to base
tcp = info['tcp'].dot(hom(ori(-10, 30, 40)))
pose = hom(tcp[:3, :3], [0.6, 0, 0.3])
s = calc_invkin_irb140(pose, raw_solutions=True)
ik_up = forward_kinematics(*s[0], **dh_table)
ik_down = forward_kinematics(*s[5], **dh_table)
ik_up_back = forward_kinematics(*s[10], **dh_table)
ik_down_back = forward_kinematics(*s[15], **dh_table)
#plot_robot_geometry(info)
plot_robot_geometry(ik_up, 'b')
plot_robot_geometry(ik_up_back, 'b--')
plot_robot_geometry(ik_down, 'r')
plot_robot_geometry(ik_down_back, 'r--')
xlabel('x [m]', fontsize=LABEL_SIZE)
ylabel('z [m]', fontsize=LABEL_SIZE)
xticks(fontsize=TICK_SIZE)
yticks(fontsize=TICK_SIZE)
grid()
axes().set_aspect('equal', 'datalim')
# save figure plot
figpath = r"C:\Users\***REMOVED***\Dropbox\exjobb\rapport\images"
savefig(path.join(figpath, "invkin-elbowupdown2.png"), bbox_inches='tight')
def setUp(self):
self.dh_table = DH_TABLE
self.dh_table['tool'] = hom(0, 0, 0, [0.1, 0.12, 0.13])
self.joints = (0, 0, 0, 0, 0, 0)
self.tcp = forward_kinematics(*self.joints, **self.dh_table)['tcp']
def pen_pos_pattern_mm_hom(self):
return nmap(lambda x: hom(0, 0, 0, x), self.pen_pos_pattern_mm_3d)
def wobj(self):
return hom(self._res[:, :3])
def tool(self):
return hom(0, 0, 0, self.tip)