Assignment 03

lecture_03/assignment_02/gabriele_mattei/assignment_02.py

@@ -0,0 +1,85 @@
+from compas.artists import Artist
+from compas.datastructures import Mesh
+from compas.geometry import Circle
+from compas.geometry import Cylinder
+from compas.geometry import Frame
+from compas.geometry import Plane
+from compas.geometry import Translation
+from compas.robots import Configuration
+from compas.robots import Joint
+from compas.robots import RobotModel
+from compas.geometry import Point
+
+# create cylinder in yz plane
+radius, length = 0.3, 5
+cylinder = Cylinder(Circle(Plane([0, 0, 0], [1, 0, 0]), radius), length)
+cylinder.transform(Translation.from_vector([length / 2.0, 0, 0]))
+
+
+# create robot model
+model = RobotModel("robot", links=[], joints=[])
+
+# link meshes (calling Mesh.from_shape effectively creates a copy of the shape)
+mesh1 = Mesh.from_shape(cylinder)
+mesh2 = Mesh.from_shape(cylinder)
+mesh3 = Mesh.from_shape(cylinder)
+mesh4 = Mesh.from_shape(cylinder)
+mesh5 = Mesh.from_shape(cylinder)
+mesh6 = Mesh.from_shape(cylinder)
+mesh7 = Mesh.from_shape(cylinder)
+mesh8 = Mesh.from_shape(cylinder)
+
+
+# add links
+link0 = model.add_link("world") #doesn't have geometry, it's just some kind of placeholder to connect to the world frame coordiante system
+link1 = model.add_link("link1", visual_mesh=mesh1, visual_color=(1, 0.0, 0.5))
+link2 = model.add_link("link2", visual_mesh=mesh2, visual_color=(0.0, 1, 0.5))
+link3 = model.add_link("link3", visual_mesh=mesh3, visual_color=(1, 0.0, 0.5))
+link4 = model.add_link("link4", visual_mesh=mesh4, visual_color=(0.0, 1, 0.5))
+link5 = model.add_link("link5", visual_mesh=mesh5, visual_color=(1, 0.0, 0.5))
+link6 = model.add_link("link6", visual_mesh=mesh6, visual_color=(0.0, 1, 0.5))
+link7 = model.add_link("link7", visual_mesh=mesh7, visual_color=(1, 0.0, 0.5))
+link8 = model.add_link("link8", visual_mesh=mesh8, visual_color=(0.0, 1, 0.5))
+
+
+
+# add joints between the links
+axis = (0, 0, 1)
+
+origin = Frame.worldXY()
+model.add_joint("joint1", Joint.CONTINUOUS, link0, link1, origin, axis)
+
+origin = Frame((length, 0, 0), (1, 0, 0), (0, 1, 0))
+model.add_joint("joint2", Joint.CONTINUOUS, link1, link2, origin, axis)
+
+origin = Frame((length, 0, 0), (1, 0, 0), (0, 1, 0))
+model.add_joint("joint3", Joint.CONTINUOUS, link2, link3, origin, axis)
+
+origin = Frame((length, 0, 0), (1, 0, 0), (0, 1, 0))
+model.add_joint("joint4", Joint.CONTINUOUS, link3, link4, origin, axis)
+
+origin = Frame((length, 0, 0), (1, 0, 0), (0, 1, 0))
+model.add_joint("joint5", Joint.CONTINUOUS, link4, link5, origin, axis)
+
+origin = Frame((length, 0, 0), (1, 0, 0), (0, 1, 0))
+model.add_joint("joint6", Joint.CONTINUOUS, link5, link6, origin, axis)
+
+origin = Frame((length, 0, 0), (1, 0, 0), (0, 1, 0))
+model.add_joint("joint7", Joint.CONTINUOUS, link6, link7, origin, axis)
+
+origin = Frame((length, 0, 0), (1, 0, 0), (0, 1, 0))
+model.add_joint("joint8", Joint.CONTINUOUS, link7, link8, origin, axis)
+
+
+# Create a configuration object matching the number of joints in your model
+configuration = model.zero_configuration()
+configuration.joint_values = joint_vals
+print(configuration)
+# Update the model using the artist
+artist = Artist(model)
+artist.update(configuration)
+
+
+# Render everything
+a = artist.draw_visual()
+artist.redraw()

lecture_04/assignment_03/gabriele_mattei/assignment_03.json

@@ -0,0 +1 @@
+[{"dtype": "compas.robots/Configuration", "value": {"joint_values": [2.5612627694430605, 0.8451018820739079, -2.1281579577416663, 5.995445063065217, -1.5707963169885275, 0.9904664420686518], "joint_types": [0, 0, 0, 0, 0, 0], "joint_names": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]}}, {"dtype": "compas.robots/Configuration", "value": {"joint_values": [2.2125640883045294, 0.8942197426093007, -2.365748140078546, 6.183917388135717, -1.5707963196109254, 0.6417677609301208], "joint_types": [0, 0, 0, 0, 0, 0], "joint_names": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]}}, {"dtype": "compas.robots/Configuration", "value": {"joint_values": [1.825801438834675, 0.9011173025596614, -2.4807665503673664, -6.274333341730043, -1.5707977101751436, 0.25500472857724027], "joint_types": [0, 0, 0, 0, 0, 0], "joint_names": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]}}, {"dtype": "compas.robots/Configuration", "value": {"joint_values": [1.4690433385290533, -5.638827005727662, -1.5560660697587136, 2.4825039565223816, 1.5707975382961925, -3.243345804591246], "joint_types": [0, 0, 0, 0, 0, 0], "joint_names": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]}}, {"dtype": "compas.robots/Configuration", "value": {"joint_values": [1.1849644619041062, -5.71908092652345, -1.3609191619728542, 2.367610697822745, 1.570796301197648, -3.527424518259446], "joint_types": [0, 0, 0, 0, 0, 0], "joint_names": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]}}, {"dtype": "compas.robots/Configuration", "value": {"joint_values": [0.9504148320944404, -5.857413808982158, -1.0398194575904207, 2.184843876420017, 1.5707962045683321, -3.7619741480690916], "joint_types": [0, 0, 0, 0, 0, 0], "joint_names": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]}}, {"dtype": "compas.robots/Configuration", "value": {"joint_values": [0.730447806935794, -6.04003485119701, -0.63229534112126, 1.9599408253541222, 1.5707961132951012, -3.9819411732277197], "joint_types": [0, 0, 0, 0, 0, 0], "joint_names": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]}}, {"dtype": "compas.robots/Configuration", "value": {"joint_values": [-1.9609183542076545, 4.0271801620205405, -1.7804900412697782, 5.607291518287111, -4.712388981510897, 5.893063279629915], "joint_types": [0, 0, 0, 0, 0, 0], "joint_names": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]}}, {"dtype": "compas.robots/Configuration", "value": {"joint_values": [-2.2151951570039348, 3.98133005957501, -1.6818990137099339, 5.554550593403931, -4.712388980274963, 5.638786476833635], "joint_types": [0, 0, 0, 0, 0, 0], "joint_names": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]}}, {"dtype": "compas.robots/Configuration", "value": {"joint_values": [-2.4614541701116415, 3.9288345994504725, -1.5695136369977738, 5.494660676737506, -4.712388979021726, 5.392527463725929], "joint_types": [0, 0, 0, 0, 0, 0], "joint_names": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]}}, {"dtype": "compas.robots/Configuration", "value": {"joint_values": [-2.693397599701714, 3.8507440761816816, -1.4030353130129796, 5.406272875662228, -4.71238897785816, 5.160584034135857], "joint_types": [0, 0, 0, 0, 0, 0], "joint_names": ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"]}}]

lecture_04/assignment_03/gabriele_mattei/assignment_03.py

@@ -0,0 +1,85 @@
+"""Assignment 03: Using inverse kinematics
+"""
+from ast import Return
+import os
+import compas
+from compas_fab.backends import RosClient
+from compas_fab.robots import Configuration
+
+from compas.geometry import Frame
+from compas.geometry import Point
+from compas.geometry import Vector
+
+
+# Step 1: Inside this function, complete the main part of the solution for the assignment:
+#  - Taking a robot and a list of frames as parameter, calculate a feasible configuration for each of the frames
+#  - Try to find an optimal start_configuration for each so that the motion from one config to the next is minimized
+
+###########
+#VERSION_0
+###########
+
+# def calculate_ik_for_frames(robot, frames):
+#     configs = []
+#     for i, f in enumerate(frames):
+#         if i == 0:
+#             start_configuration = robot.zero_configuration()
+#             config_0 = robot.inverse_kinematics(f, start_configuration)
+#             configs.append(config_0)
+#         else:
+#             start_configuration = config_0
+#             config_new = robot.inverse_kinematics(f, start_configuration)
+#             configs.append(config_new)
+
+#     return configs
+
+############
+# VERSION_1
+############
+
+def calculate_ik_for_frames(robot, frames):
+    configs = []
+    start_configuration = robot.zero_configuration() 
+    for f in frames:
+        config = robot.inverse_kinematics(f, start_configuration)
+        configs.append(config)
+        start_configuration = config
+
+    return configs
+
+
+# Step 2: store all found configurations in a JSON file using compas.json_dump or compas.json_dumps
+def store_configurations(configurations, filename):
+    compas.json_dump(configurations, filename)
+
+
+# Use the following to test from the command line
+# Or copy solution_viewer.ghx next to the folder where you created assignment_03.py to visualize the same in Grasshopper
+if __name__ == '__main__':
+
+    frames = [
+        Frame(Point(-0.329, 0.059, 0.082), Vector(1.000, 0.000, 0.000), Vector(0.000, -1.000, 0.000)),
+        Frame(Point(-0.260, 0.129, 0.082), Vector(1.000, 0.000, 0.000), Vector(0.000, -1.000, 0.000)),
+        Frame(Point(-0.186, 0.194, 0.082), Vector(1.000, 0.000, 0.000), Vector(0.000, -1.000, 0.000)),
+        Frame(Point(-0.106, 0.252, 0.082), Vector(1.000, 0.000, 0.000), Vector(0.000, -1.000, 0.000)),
+        Frame(Point(-0.020, 0.299, 0.082), Vector(1.000, 0.000, 0.000), Vector(0.000, -1.000, 0.000)),
+        Frame(Point(0.074, 0.329, 0.082), Vector(1.000, 0.000, 0.000), Vector(0.000, -1.000, 0.000)),
+        Frame(Point(0.172, 0.330, 0.082), Vector(1.000, 0.000, 0.000), Vector(0.000, -1.000, 0.000)),
+        Frame(Point(0.263, 0.295, 0.082), Vector(1.000, 0.000, 0.000), Vector(0.000, -1.000, 0.000)),
+        Frame(Point(0.339, 0.233, 0.082), Vector(1.000, 0.000, 0.000), Vector(0.000, -1.000, 0.000)),
+        Frame(Point(0.400, 0.155, 0.082), Vector(1.000, 0.000, 0.000), Vector(0.000, -1.000, 0.000)),
+        Frame(Point(0.448, 0.070, 0.082), Vector(1.000, 0.000, 0.000), Vector(0.000, -1.000, 0.000))]
+
+
+    # Loads the robot from ROS
+    with RosClient('localhost') as client:
+        robot = client.load_robot()
+
+        # Step 1: calculate IK solutions for each frame
+        configurations = calculate_ik_for_frames(robot, frames)
+        print("Found {} configurations".format(len(configurations)))
+
+        # Step 2: store all configurations in a JSON file
+        filename = os.path.join(os.path.abspath(os.path.dirname(__file__)), 'assignment_03.json')
+        store_configurations(configurations, filename)
+        print("Stored results in {}".format(filename))