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)