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()