def set_base(self, base_frame): # move to UR !!!! self.base_frame = base_frame # transformation matrix from world coordinate system to robot coordinate system self.transformation_WCS_RCS = Transformation.from_frame_to_frame( Frame.worldXY(), self.base_frame) # transformation matrix from robot coordinate system to world coordinate system self.transformation_RCS_WCS = Transformation.from_frame_to_frame( self.base_frame, Frame.worldXY())
def __init__(self): self.model = [] # a list of meshes self.model_loaded = False self.basis_frame = None # move to UR !!!! self.transformation_RCS_WCS = None self.transformation_WCS_RCS = None self.set_base(Frame.worldXY()) self.tool = Tool(Frame.worldXY()) self.configuration = None self.tool0_frame = Frame.worldXY()
def __init__(self): super(UR, self).__init__() d1, a2, a3, d4, d5, d6 = self.params # j0 - j5 are the axes around which the joints m0 - m5 rotate, e.g. m0 # rotates around j0, m1 around j1, etc. self.j0 = [(0, 0, 0), (0, 0, d1)] self.j1 = [(0, 0, d1), (0, -self.shoulder_offset, d1)] self.j2 = [(a2, -self.shoulder_offset-self.elbow_offset, d1), (a2, -self.shoulder_offset, d1)] self.j3 = [(a2+a3, 0, d1), (a2+a3,-d4, d1)] self.j4 = [(a2+a3, -d4, d1), (a2+a3, -d4, d1-d5)] self.j5 = [(a2+a3, -d4, d1-d5), (a2+a3, -d4-d6, d1-d5)] self.tool0_frame = Frame(self.j5[1], [1,0,0], [0,0,1])
def forward_kinematics(configuration, ur_params): """Forward kinematics function. This is the wrapper for the forward kinematics function from ROS. Our robots somehow differ to the standard configuration. Therefore we need to swap angles and rotate the first joint by -pi. (The initial position can be visualized by loading the meshes.) Args: configuration, the 6 joint angles in radians ur_params: UR defined parameters for the model Returns: the frame """ configuration[0] += math.pi T = forward_ros(configuration, ur_params) xaxis = [T[1], T[5], T[9]] yaxis = [T[2], T[6], T[10]] point = [T[3], T[7], T[11]] return Frame(point, xaxis, yaxis)
def get_tool0_frame_from_tcp_frame(self, frame_tcp): """Get the tool0 frame (frame at robot) from the tool frame (tcp), according to the set tool. """ T = Transformation.from_frame(frame_tcp) return Frame.from_transformation(T * self.transformation_tool0_tcp)
def __init__(self, tcp_frame): self.model = None self.tool0_frame = Frame.worldXY() self.tcp_frame = tcp_frame # TODO: Gonzalo: I find the single-letter part of the name a bit confusing. Maybe rename this to # self.transformation_to_tool0 and self.transformation_from_tool0 (or similar)? self.transformation_T0_T = Transformation.from_frame_to_frame(self.tcp_frame, self.tool0_frame) self.transformation_T_T0 = Transformation.from_frame_to_frame(self.tool0_frame, self.tcp_frame)
the frame """ configuration[0] += math.pi T = forward_ros(configuration, ur_params) xaxis = [T[1], T[5], T[9]] yaxis = [T[2], T[6], T[10]] point = [T[3], T[7], T[11]] return Frame(point, xaxis, yaxis) if __name__ == "__main__": #frame = Frame([56.9907, 410.9482, 432.3825], [0.0000, 1.0000, 0.0000], [1.0000, 0.0000, 0.0000]) frame = Frame([110.9482, -243.0093, -432.3825], [1.0000, 0.0000, 0.0000], [0.0000, 1.0000, 0.0000]) ur_params = [89.159, -425.0, -392.25, 109.15, 94.65, 82.3] qsols = inverse_kinematics(frame, ur_params) for q in qsols: print forward_kinematics(q, ur_params) q = [ -0.4817717618752444, 2.900620189456401, 4.466606474692679, 3.6283476234151966, 1.5707963267948974, 5.194160742259934 ]
return self.data @property def data(self): """:obj:`dict` : The data representing the pose.""" return {'values': self.values} @data.setter def data(self, data): values = data.get('values') or None if len(values) == 12: values.extend([0., 0., 0., 1.]) elif len(values) != 16: raise ValueError('Expected 12 or 16 floats but got %d' % len(values)) self.values = values if __name__ == "__main__": base_frame = Frame([-636.57, 370.83, 293.21], [0.00000, -0.54972, -0.83535], [0.92022, -0.32695, 0.21516]) robot = Robot() robot.set_base(base_frame) T1 = robot.transformation_WCS_RCS T2 = robot.transformation_RCS_WCS print(T1 * T2) print(robot.transformation_tcp_tool0)
class UR(Robot): """The UR robot class. """ d1 = 0 a2 = 0 a3 = 0 d4 = 0 d5 = 0 d6 = 0 shoulder_offset = 0 elbow_offset = 0 def __init__(self): super(UR, self).__init__() d1, a2, a3, d4, d5, d6 = self.params # j0 - j5 are the axes around which the joints m0 - m5 rotate, e.g. m0 # rotates around j0, m1 around j1, etc. self.j0 = [(0, 0, 0), (0, 0, d1)] self.j1 = [(0, 0, d1), (0, -self.shoulder_offset, d1)] self.j2 = [(a2, -self.shoulder_offset-self.elbow_offset, d1), (a2, -self.shoulder_offset, d1)] self.j3 = [(a2+a3, 0, d1), (a2+a3,-d4, d1)] self.j4 = [(a2+a3, -d4, d1), (a2+a3, -d4, d1-d5)] self.j5 = [(a2+a3, -d4, d1-d5), (a2+a3, -d4-d6, d1-d5)] self.tool0_frame = Frame(self.j5[1], [1,0,0], [0,0,1]) @property def params(self): """Get UR specific model parameters. Returns: list: UR specific model parameters. """ return [self.d1, self.a2, self.a3, self.d4, self.d5, self.d6] def get_model_path(self): pass def load_model(self): """Load the geometry (meshes) of the robot. """ path = self.get_model_path() # the joints loaded as meshes self.m0 = Mesh.from_obj(os.path.join(path, 'base_and_shoulder.obj')) self.m1 = Mesh.from_obj(os.path.join(path, 'upperarm.obj')) self.m2 = Mesh.from_obj(os.path.join(path, 'forearm.obj')) self.m3 = Mesh.from_obj(os.path.join(path, 'wrist1.obj')) self.m4 = Mesh.from_obj(os.path.join(path, 'wrist2.obj')) self.m5 = Mesh.from_obj(os.path.join(path, 'wrist3.obj')) # have a copy of the mesh vertices for later transformation self.m0_xyz = self.m0.xyz self.m1_xyz = self.m1.xyz self.m2_xyz = self.m2.xyz self.m3_xyz = self.m3.xyz self.m4_xyz = self.m4.xyz self.m5_xyz = self.m5.xyz def get_forward_transformations(self, joint_angles): def vector(line): start, end = line return [end[0] - start[0], end[1] - start[1], end[2] - start[2]] q0, q1, q2, q3, q4, q5 = joint_angles j0, j1, j2, j3, j4, j5 = self.j0, self.j1, self.j2, self.j3, self.j4, self.j5 R0 = Rotation.from_axis_and_angle(vector(j0), q0, j0[1]) j1 = [R0.transform(j1[0]), R0.transform(j1[1])] R1 = Rotation.from_axis_and_angle(vector(j1), q1, j1[1]) * R0 j2 = [R1.transform(j2[0]), R1.transform(j2[1])] R2 = Rotation.from_axis_and_angle(vector(j2), q2, j2[1]) * R1 j3 = [R2.transform(j3[0]), R2.transform(j3[1])] R3 = Rotation.from_axis_and_angle(vector(j3), q3, j3[1]) * R2 j4 = [R3.transform(j4[0]), R3.transform(j4[1])] R4 = Rotation.from_axis_and_angle(vector(j4), q4, j4[1]) * R3 j5 = [R4.transform(j5[0]), R4.transform(j5[1])] R5 = Rotation.from_axis_and_angle(vector(j5), q5, j5[1]) * R4 # now apply the transformation to the base R0 = self.transformation_RCS_WCS * R0 R1 = self.transformation_RCS_WCS * R1 R2 = self.transformation_RCS_WCS * R2 R3 = self.transformation_RCS_WCS * R3 R4 = self.transformation_RCS_WCS * R4 R5 = self.transformation_RCS_WCS * R5 return R0, R1, R2, R3, R4, R5 def get_transformed_tool_frames(self, R5): tool0_frame = self.tool0_frame.transform(R5, copy=True) tcp_frame = Frame.from_transformation(Transformation.from_frame(tool0_frame) * self.transformation_tool0_tcp) return tool0_frame, tcp_frame def get_transformed_model(self, q): """Calculate robot model according to the configuration. Args: configuration (Configuration): the 6 joint angles in radians Returns: (frame): The tool0 frame in robot coordinate system (RCS). Get the transformed meshes of the robot model. """ R0, R1, R2, R3, R4, R5 = self.get_forward_transformations(q) tool0_frame, tcp_frame = self.get_transformed_tool_frames(R5) # transform the original vertices, rather than the mesh.vertices, # otherwise the already transformed vertices are transformed m0_xyz = R0.transform(self.m0_xyz) m1_xyz = R1.transform(self.m1_xyz) m2_xyz = R2.transform(self.m2_xyz) m3_xyz = R3.transform(self.m3_xyz) m4_xyz = R4.transform(self.m4_xyz) m5_xyz = R5.transform(self.m5_xyz) # update the meshes mesh_update_vertices(self.m0, m0_xyz) mesh_update_vertices(self.m1, m1_xyz) mesh_update_vertices(self.m2, m2_xyz) mesh_update_vertices(self.m3, m3_xyz) mesh_update_vertices(self.m4, m4_xyz) mesh_update_vertices(self.m5, m5_xyz) return self.m0, self.m1, self.m2, self.m3, self.m4, self.m5, tool0_frame, tcp_frame def get_transformed_tool_model(self, tcp_frame): return self.tool.get_transformed_tool_model(tcp_frame) def forward_kinematics(self, configuration): """Forward kinematics function. Args: configuration (Configuration): the 6 joint angles in radians Returns: (frame): The tool0 frame in robot coordinate system (RCS). """ return forward_kinematics(configuration, self.params) def inverse_kinematics(self, tool0_frame_RCS): """Inverse kinematics function. Args: tool0_frame_RCS (Frame): The tool0 frame to reach in robot coordinate system (RCS). Returns: list: A list of possible configurations. """ return inverse_kinematics(tool0_frame_RCS, self.params)
def get_transformed_tool_frames(self, R5): tool0_frame = self.tool0_frame.transform(R5, copy=True) tcp_frame = Frame.from_transformation(Transformation.from_frame(tool0_frame) * self.transformation_tool0_tcp) return tool0_frame, tcp_frame
def __init__(self): origin = [0.0, 0.0, 115.6] xaxis = [0.0, 1., 0.0] yaxis = [1., 0.0, 0.0] super(MeasurementTool, self).__init__(Frame(origin, xaxis, yaxis)) self.load_model()