Assignment - 03

lecture_04/assignment_03/Wenjun Liu/assignment-03.json

@@ -0,0 +1 @@
+[{"dtype": "compas.robots/Configuration", "value": {"joint_values": [-6.0636094922272195, 4.187176626902659, -2.125189870934173, 5.792009185209975, -4.696058345228733, 1.79790631238704], "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.01240008447220296, 4.2845706699419654, -2.370814973134062, 5.948072284989481, -4.729403371100378, 1.582326790417698], "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.29753737009962045, 4.343436213133126, -2.481419734085892, 5.986300862029157, -4.716036524726565, 1.272674697787544], "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": [5.609826463493053, 4.324444360421428, -2.4591374643863033, 6.001442647284475, -4.703762012652165, 0.8809427602808998], "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": [5.233071224265763, 4.268550244309271, -2.316820106064135, 5.899165847797696, -4.7210522518758244, 0.5090542698460556], "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.3895585088618847, 4.181933924916302, -2.1084702086757003, 5.7728162036184685, -4.719709729779042, 0.19132727363993843], "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.684754072962214, 4.0915734901061995, -1.916108822054683, 5.6694050679286985, -4.729893745355812, -0.11325806613479349], "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.960437609016056, 4.0271115729347144, -1.7866569801980183, 5.6173616326538935, -4.7182649889008355, -0.38616564189431113], "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.2127708092765475, -2.2983087589668427, -1.6849433877197537, 5.553794274665422, -4.720614852527842, -0.6247421191834107], "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.457806605528404, 3.927959723211486, -1.5757451111987182, 5.50923817423025, -4.73001918384491, -0.873957860364332], "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.6950471006081744, 3.8498881334835455, -1.3951662205573625, 5.393049462281264, -4.713467571224014, -1.1263691673269804], "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/Wenjun Liu/assignment_03.py

@@ -0,0 +1,94 @@
+"""Assignment 03: Using inverse kinematics
+"""
+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
+
+import math
+
+# 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
+def calculate_ik_for_frames(robot, frames):
+    # 1. Used inverse_kinematics with set start_configuration, but found it still jumping randomly from frames
+    """
+    configs = []
+    start_config = robot.zero_configuration()
+    start_config.joint_values =(-6.058, 4.186, -2.128, 5.796, -4.712, 1.796)
+    for frame in frames:
+        config = robot.inverse_kinematics(frame, start_config)
+        configs.append(config)
+        start_config = config
+    return configs
+    """
+
+    # 2. Searching for better control to minimze the motion through motion plan
+    configs = []
+    group = robot.main_group_name
+    tolerance_position = 0.001
+    tolerance_axes = [math.radians(1)] * 3
+    start_configuration = robot.zero_configuration()
+    start_configuration.joint_values = (-6.058, 4.186, -2.128, 5.796, -4.712, 1.796)
+
+    for frame in frames:
+        goal_constraints = robot.constraints_from_frame(frame, tolerance_position, tolerance_axes, group)
+        config_check = []
+        motion_steps = []
+
+        # collect possible motion paths
+        for i in range(100): # Unstable results for 20 or 50, 100 should be satisfactory
+            trajectory = robot.plan_motion(goal_constraints, start_configuration, group, options=dict(planner_id="RRT"))
+            motion_step = len(trajectory.points)
+            motion_steps.append(motion_step)
+            config = robot.zero_configuration()
+            config.joint_values = trajectory.points[-1].joint_values
+            config_check.append(config)
+
+        # pick path with minimun motion
+        minimun_step_id = motion_steps.index(min(motion_steps))
+        print(min(motion_steps))
+        config = config_check[minimun_step_id]
+        start_configuration = config
+        configs.append(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)
+    pass
+
+# 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))