planner_id='RRT',
num_planning_attempts=20,
allowed_planning_time=8.,
attached_collision_object=aco)
paths.append(response.trajectory)
last_configuration = response.configurations[-1]
last_configuration = robot.merge_group_with_full_configuration(last_configuration, picking_configuration, group)
except RosError as error:
print(error)
break
print("last_configuration", last_configuration)
# Calculate cartesian path between saveframe_place and placing_frame
placing_frame_tolerance = placing_frame.copy()
placing_frame_tolerance.point += tolerance_vector
frames = [saveframe_place, placing_frame_tolerance]
try:
response = robot.compute_cartesian_path(frames_WCF=frames,
start_configuration=last_configuration,
max_step=0.01,
avoid_collisions=True,
group=group,
path_constraints=None,
attached_collision_object=aco)
if response.fraction == 1.:
paths.append(response.solution)
last_configuration = response.configurations[-1]
else:
element_tool0 = element.copy()
T = Transformation.from_frame_to_frame(element_tool0.gripping_frame,
tool.frame)
element_tool0.transform(T)
# define picking_configuration
picking_configuration = Configuration.from_data(data['picking_configuration'])
# define picking frame
picking_frame = Frame.from_data(data['picking_frame'])
picking_frame.point += tolerance_vector
# define savelevel frames 'above' the picking- and target frames
savelevel_picking_frame = picking_frame.copy()
savelevel_picking_frame.point += savelevel_vector
savelevel_target_frame = target_frame.copy()
savelevel_target_frame.point += savelevel_vector
# settings for plan_motion
tolerance_position = 0.001
tolerance_axes = [math.radians(1)] * 3
with RosClient('localhost') as client:
robot = client.load_robot()
scene = PlanningScene(robot)
scene.remove_collision_mesh('brick_wall')
# attach tool
robot.attach_tool(tool)
# add tool to scene
def inverse_kinematics_spherical_wrist(p1, p2, p3, p4, target_frame):
"""Inverse kinematics function for spherical wrist robots.
This is a modification of the *Lobster* tool for solving the inverse
kinematics problem for a spherical wrist robots.
https://www.grasshopper3d.com/forum/topics/lobster-reloaded?groupUrl=lobster&
This is the instruction on how to determine the 4 points.
https://api.ning.com/files/KRgE1yt2kgG2IF9H8sre4CWfIDL9ytv5WvVn54zdOx6HE84gDordaHzo0jqwP-Qhry7MyRQ4IQxY1p3cIkqEDj1FAVVR2Xg0/Lobster_IK.pdf
check p2, p3, p4 must be in one XY plane!
"""
end_frame = Frame(target_frame.point, target_frame.yaxis,
target_frame.xaxis)
wrist_offset = p4.x - p3.x
lower_arm_length = p2.z - p1.z
upper_arm_length = (p3 - p2).length
pln_offset = math.fabs(p2.y - p1.y)
axis4_offset_angle = math.atan2(p3.z - p2.z, p3.x - p2.x)
wrist = end_frame.to_world_coordinates(Point(0, 0, wrist_offset))
p1_proj = p1.copy()
p1_proj.y = p2.y
axis1_angles = []
axis2_angles = []
axis3_angles = []
axis4_angles = []
axis5_angles = []
axis6_angles = []
if pln_offset == 0:
axis1_angle = -1 * math.atan2(wrist.y, wrist.x)
if axis1_angle > math.pi:
axis1_angle -= 2 * math.pi
axis1_angles += [axis1_angle] * 4
axis1_angle += math.pi
if axis1_angle > math.pi:
axis1_angle -= 2 * math.pi
axis1_angles += [axis1_angle] * 4
else:
circle = (0, 0, 0), pln_offset
point = (wrist.x, wrist.y, 0)
t1, t2 = tangent_points_to_circle_xy(circle, point)
a1 = Vector(0, 1, 0).angle_signed(t1, (0, 0, -1))
a2 = Vector(0, 1, 0).angle_signed(t2, (0, 0, -1))
axis1_angles += [a1] * 4
axis1_angles += [a2] * 4
for i in range(2):
axis1_angle = axis1_angles[i * 4]
Rot1 = Rotation.from_axis_and_angle([0, 0, 1],
-1 * axis1_angle,
point=[0, 0, 0])
p1A = p1_proj.transformed(Rot1)
elbow_dir = Vector(1, 0, 0).transformed(Rot1)
sphere1 = Sphere(p1A, lower_arm_length)
sphere2 = Sphere(wrist, upper_arm_length)
elbow_frame = Frame(p1A, elbow_dir, [0, 0, 1])
elbow_plane = (p1A, elbow_frame.normal)
result = intersection_sphere_sphere(sphere1, sphere2)
if result is not None:
case, (center, radius, normal) = result
else:
return None
circle = ((center, normal), radius)
intersect_pt1, intersect_pt2 = intersection_plane_circle(
elbow_plane, circle)
for j in range(2):
if j == 0:
elbow_pt = intersect_pt1
else:
elbow_pt = intersect_pt2
elbow_pt = Point(*elbow_pt)
elbowx, elbowy, elbowz = elbow_frame.to_local_coordinates(elbow_pt)
wristx, wristy, wristz = elbow_frame.to_local_coordinates(wrist)
axis2_angle = math.atan2(elbowy, elbowx)
axis3_angle = math.pi - axis2_angle + math.atan2(
wristy - elbowy, wristx - elbowx) - axis4_offset_angle
for k in range(2):
axis2_angles.append(-axis2_angle)
axis3_angle_wrapped = -axis3_angle + math.pi
while (axis3_angle_wrapped >= math.pi):
axis3_angle_wrapped -= 2 * math.pi
while (axis3_angle_wrapped < -math.pi):
axis3_angle_wrapped += 2 * math.pi
axis3_angles.append(axis3_angle_wrapped)
for k in range(2):
axis4 = wrist - elbow_pt
axis4.transform(
Rotation.from_axis_and_angle(elbow_frame.zaxis,
-axis4_offset_angle))
temp_frame = elbow_frame.copy() # copy yes or no?
temp_frame.transform(
Rotation.from_axis_and_angle(temp_frame.zaxis,
axis2_angle + axis3_angle))
# // B = TempPlane
axis4_frame = Frame(wrist, temp_frame.zaxis,
temp_frame.yaxis * -1.0)
axis6x, axis6y, axis6z = axis4_frame.to_local_coordinates(
end_frame.point)
axis4_angle = math.atan2(axis6y, axis6x)
if k == 1:
axis4_angle += math.pi
if axis4_angle > math.pi:
axis4_angle -= 2 * math.pi
axis4_angle_wrapped = axis4_angle + math.pi / 2
while (axis4_angle_wrapped >= math.pi):
axis4_angle_wrapped -= 2 * math.pi
while (axis4_angle_wrapped < -math.pi):
axis4_angle_wrapped += 2 * math.pi
axis4_angles.append(axis4_angle_wrapped)
axis5_frame = axis4_frame.copy()
axis5_frame.transform(
Rotation.from_axis_and_angle(axis4_frame.zaxis,
axis4_angle))
axis5_frame = Frame(wrist, axis5_frame.zaxis * -1,
axis5_frame.xaxis)
axis6x, axis6y, axis6z = axis5_frame.to_local_coordinates(
end_frame.point)
axis5_angle = math.atan2(axis6y, axis6x)
axis5_angles.append(axis5_angle)
axis6_frame = axis5_frame.copy()
axis6_frame.transform(
Rotation.from_axis_and_angle(axis5_frame.zaxis,
axis5_angle))
axis6_frame = Frame(wrist, axis6_frame.yaxis * -1,
axis6_frame.zaxis)
endx, endy, endz = axis6_frame.to_local_coordinates(
end_frame.to_world_coordinates(Point(1, 0, 0)))
axis6_angle = math.atan2(endy, endx)
axis6_angles.append(axis6_angle)
def inverse_kinematics_spherical_wrist(target_frame, points):
"""Inverse kinematics function for spherical wrist robots.
Parameters
----------
frame : :class:`compas.geometry.Frame`
The frame we search the inverse kinematics for.
points : list of point
A list of 4 points specifying the robot's joint positions.
Returns
-------
list of list of float
The 8 analytical IK solutions.
Notes
-----
This is a modification of the *Lobster* tool for solving the inverse
kinematics problem for a spherical wrist robots.
https://www.grasshopper3d.com/forum/topics/lobster-reloaded?groupUrl=lobster&
This is the instruction on how to determine the 4 points.
http://archive.fabacademy.org/archives/2017/fablabcept/students/184/assets/lobster_ik.pdf
Check that p2, p3, and p4 are all in the XZ plane (y coordinates are zero)!
"""
p1, p2, p3, p4 = points
end_frame = Frame(target_frame.point, target_frame.yaxis,
target_frame.xaxis)
wrist_offset = p4.x - p3.x
lower_arm_length = p2.z - p1.z
upper_arm_length = (p3 - p2).length
pln_offset = math.fabs(p2.y - p1.y)
axis4_offset_angle = math.atan2(p3.z - p2.z, p3.x - p2.x)
wrist = end_frame.to_world_coordinates(Point(0, 0, wrist_offset))
p1_proj = p1.copy()
p1_proj.y = p2.y
axis1_angles = []
axis2_angles = []
axis3_angles = []
axis4_angles = []
axis5_angles = []
axis6_angles = []
if pln_offset == 0:
axis1_angle = -1 * math.atan2(wrist.y, wrist.x)
if axis1_angle > math.pi:
axis1_angle -= 2 * math.pi
axis1_angles += [axis1_angle] * 4
axis1_angle += math.pi
if axis1_angle > math.pi:
axis1_angle -= 2 * math.pi
axis1_angles += [axis1_angle] * 4
else:
circle = (0, 0, 0), pln_offset
point = (wrist.x, wrist.y, 0)
t1, t2 = tangent_points_to_circle_xy(circle, point)
a1 = Vector(0, 1, 0).angle_signed(t1, (0, 0, -1))
a2 = Vector(0, 1, 0).angle_signed(t2, (0, 0, -1))
axis1_angles += [a1] * 4
axis1_angles += [a2] * 4
for i in range(2):
axis1_angle = axis1_angles[i * 4]
Rot1 = Rotation.from_axis_and_angle([0, 0, 1],
-1 * axis1_angle,
point=[0, 0, 0])
p1A = p1_proj.transformed(Rot1)
elbow_dir = Vector(1, 0, 0).transformed(Rot1)
sphere1 = Sphere(p1A, lower_arm_length)
sphere2 = Sphere(wrist, upper_arm_length)
elbow_frame = Frame(p1A, elbow_dir, [0, 0, 1])
elbow_plane = (p1A, elbow_frame.normal)
_, (center, radius,
normal) = intersection_sphere_sphere(sphere1, sphere2)
circle = ((center, normal), radius)
intersect_pt1, intersect_pt2 = intersection_plane_circle(
elbow_plane, circle)
for j in range(2):
if j == 0:
elbow_pt = intersect_pt1
else:
elbow_pt = intersect_pt2
elbow_pt = Point(*elbow_pt)
elbowx, elbowy, _ = elbow_frame.to_local_coordinates(elbow_pt)
wristx, wristy, _ = elbow_frame.to_local_coordinates(wrist)
axis2_angle = math.atan2(elbowy, elbowx)
axis3_angle = math.pi - axis2_angle + math.atan2(
wristy - elbowy, wristx - elbowx) - axis4_offset_angle
for k in range(2):
axis2_angles.append(-axis2_angle)
axis3_angle_wrapped = -axis3_angle + math.pi
while (axis3_angle_wrapped >= math.pi):
axis3_angle_wrapped -= 2 * math.pi
while (axis3_angle_wrapped < -math.pi):
axis3_angle_wrapped += 2 * math.pi
axis3_angles.append(axis3_angle_wrapped)
for k in range(2):
axis4 = wrist - elbow_pt
axis4.transform(
Rotation.from_axis_and_angle(elbow_frame.zaxis,
-axis4_offset_angle))
temp_frame = elbow_frame.copy()
temp_frame.transform(
Rotation.from_axis_and_angle(temp_frame.zaxis,
axis2_angle + axis3_angle))
# // B = TempPlane
axis4_frame = Frame(wrist, temp_frame.zaxis,
temp_frame.yaxis * -1.0)
axis6x, axis6y, axis6z = axis4_frame.to_local_coordinates(
end_frame.point)
axis4_angle = math.atan2(axis6y, axis6x)
if k == 1:
axis4_angle += math.pi
if axis4_angle > math.pi:
axis4_angle -= 2 * math.pi
axis4_angle_wrapped = axis4_angle + math.pi / 2
while (axis4_angle_wrapped >= math.pi):
axis4_angle_wrapped -= 2 * math.pi
while (axis4_angle_wrapped < -math.pi):
axis4_angle_wrapped += 2 * math.pi
axis4_angles.append(axis4_angle_wrapped)
axis5_frame = axis4_frame.copy()
axis5_frame.transform(
Rotation.from_axis_and_angle(axis4_frame.zaxis,
axis4_angle))
axis5_frame = Frame(wrist, axis5_frame.zaxis * -1,
axis5_frame.xaxis)
axis6x, axis6y, _ = axis5_frame.to_local_coordinates(
end_frame.point)
axis5_angle = math.atan2(axis6y, axis6x)
axis5_angles.append(axis5_angle)
axis6_frame = axis5_frame.copy()
axis6_frame.transform(
Rotation.from_axis_and_angle(axis5_frame.zaxis,
axis5_angle))
axis6_frame = Frame(wrist, axis6_frame.yaxis * -1,
axis6_frame.zaxis)
endx, endy, _ = axis6_frame.to_local_coordinates(
end_frame.to_world_coordinates(Point(1, 0, 0)))
axis6_angle = math.atan2(endy, endx)
axis6_angles.append(axis6_angle)
return [[a1, a2, a3, a4, a5, a6] for a1, a2, a3, a4, a5, a6 in zip(
axis1_angles, axis2_angles, axis3_angles, axis4_angles, axis5_angles,
axis6_angles)]
class Connection(FromToData, FromToJson, FromToPickle, Datastructure):
"""Wasp Connection
"""
__module__ = 'compas_wasp'
def __init__(self, _frame=None, _type=None, _part=None, _id=None):
super(Connection, self).__init__()
self.frame = _frame
self.flip_pln = None
if self.frame is not None:
flip_pln_Y = self.frame.yaxis.copy()
flip_pln_Y.scale(-1)
self.flip_pln = Frame(self.frame.point, self.frame.xaxis,
flip_pln_Y)
self.type = _type
self.part = _part
self.id = _id
self.rules_table = []
self.active_rules = []
## override Rhino .ToString() method (display name of the class in Gh)
def ToString(self):
return "WaspConnection [id: %s, type: %s]" % (self.id, self.type)
@property
def data(self):
"""dict : A data dict representing the part data structure for serialisation.
The dict has the following structure:
* 'aaa' => dict
Note
----
All dictionary keys are converted to their representation value (``repr(key)``)
to ensure compatibility of all allowed key types with the JSON serialisation
format, which only allows for dict keys that are strings.
"""
data = {
'f_origin':
[self.frame.point.x, self.frame.point.y, self.frame.point.z],
'f_xaxis':
[self.frame.xaxis.x, self.frame.xaxis.y, self.frame.xaxis.z],
'f_yaxis':
[self.frame.yaxis.x, self.frame.yaxis.y, self.frame.yaxis.z],
'type':
self.type,
'part':
self.part,
'id':
str(self.id)
}
return data
@data.setter
def data(self, data):
f_origin = data.get('f_origin') or []
f_xaxis = data.get('f_xaxis') or []
f_yaxis = data.get('f_yaxis') or []
c_type = data.get('type') or None
c_part = data.get('part') or None
c_id = data.get('id') or None
self.frame = Frame(f_origin, f_xaxis, f_yaxis)
flip_pln_Y = self.frame.yaxis.copy()
flip_pln_Y.scale(-1)
self.flip_pln = Frame(self.frame.point, self.frame.xaxis, flip_pln_Y)
self.type = c_type
self.part = c_part
self.id = int(c_id)
self.rules_table = []
self.active_rules = []
## return a transformed copy of the connection
def transform(self, trans):
pln_trans = self.frame.transformed(trans)
conn_trans = Connection(pln_trans, self.type, self.part, self.id)
return conn_trans
## return a copy of the connection
def copy(self):
pln_copy = self.frame.copy()
conn_copy = Connection(pln_copy, self.type, self.part, self.id)
return conn_copy
## generate the rules-table for the connection
def generate_rules_table(self, rules):
count = 0
self.rules_table = []
self.active_rules = []
for rule in rules:
if rule.part1 == self.part and rule.conn1 == self.id:
self.rules_table.append(rule)
self.active_rules.append(count)
count += 1
