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