├── Kinematics ├── 6DoF_Box.py ├── Demo │ ├── Collision_Demo.mp4 │ ├── Has_Collision.png │ └── No_Collision.png ├── collision_boxes.py ├── franka_robot.py ├── franka_robot.urdf ├── franka_visualization.rviz ├── joint_trajectory.npy ├── requirements.txt ├── test_collision_boxes.py ├── test_collision_detection.py ├── test_forward_kinematics.py ├── test_inverse_kinematics.py ├── test_jacobian.py ├── visualize.launch └── writeup │ └── Kinematics_Pandas.pdf ├── LICENSE ├── Path Planning ├── README.md ├── collision_boxes_publisher.py ├── demo │ ├── ConstrainedRRT.mp4 │ └── RRT.mp4 ├── franka_robot.py ├── franka_visualization.rviz ├── kdtree.py ├── requirements.txt ├── rrt.py ├── rrt_connect.py ├── test_rrt.py ├── test_rrt_connect.py ├── visualize.launch └── writeup │ ├── PathPlanning_Pandas.pdf │ └── PathPlanning_wirteup.pdf ├── README.md └── images ├── ConstrainedExtend.png └── franka.png /Kinematics/6DoF_Box.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Copyright (c) 2011, Willow Garage, Inc. 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of the Willow Garage, Inc. nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | SUBSTITUTE GOODS OR SERVICES LOSS OF USE, DATA, OR PROFITS OR BUSINESS 26 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import rospy 33 | import copy 34 | 35 | from interactive_markers.interactive_marker_server import * 36 | from interactive_markers.menu_handler import * 37 | from visualization_msgs.msg import * 38 | from geometry_msgs.msg import Point 39 | 40 | from math import sin 41 | 42 | server = None 43 | menu_handler = MenuHandler() 44 | 45 | def processFeedback( feedback ): 46 | s = "Feedback from marker '" + feedback.marker_name 47 | s += "' / control '" + feedback.control_name + "'" 48 | 49 | mp = "" 50 | if feedback.mouse_point_valid: 51 | mp = " at " + str(feedback.mouse_point.x) 52 | mp += ", " + str(feedback.mouse_point.y) 53 | mp += ", " + str(feedback.mouse_point.z) 54 | mp += " in frame " + feedback.header.frame_id 55 | 56 | if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: 57 | rospy.loginfo( s + ": button click" + mp + "." ) 58 | elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: 59 | rospy.loginfo( s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + "." ) 60 | elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: 61 | rospy.loginfo( s + ": pose changed") 62 | elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: 63 | rospy.loginfo( s + ": mouse down" + mp + "." ) 64 | elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: 65 | rospy.loginfo( s + ": mouse up" + mp + "." ) 66 | 67 | server.applyChanges() 68 | 69 | def makeBox( msg ): 70 | marker = Marker() 71 | 72 | marker.type = Marker.CUBE 73 | marker.scale.x = msg.scale * 0.5 74 | marker.scale.y = msg.scale * 0.3 75 | marker.scale.z = msg.scale * 0.42 76 | marker.color.r = 0.5 77 | marker.color.g = 0.5 78 | marker.color.b = 0.5 79 | marker.color.a = 1.0 80 | 81 | return marker 82 | 83 | def makeBoxControl( msg ): 84 | control = InteractiveMarkerControl() 85 | control.always_visible = True 86 | control.markers.append( makeBox(msg) ) 87 | msg.controls.append( control ) 88 | return control 89 | 90 | 91 | ##################################################################### 92 | # Marker Creation 93 | 94 | def make6DofMarker( fixed, interaction_mode, position, show_6dof = False): 95 | int_marker = InteractiveMarker() 96 | int_marker.header.frame_id = "panda_link0" 97 | int_marker.pose.position = position 98 | int_marker.scale = 0.3 99 | 100 | int_marker.name = "simple_6dof" 101 | int_marker.description = "Simple 6-DOF Control" 102 | 103 | # insert a box 104 | makeBoxControl(int_marker) 105 | int_marker.controls[0].interaction_mode = interaction_mode 106 | 107 | if fixed: 108 | int_marker.name += "_fixed" 109 | int_marker.description += "\n(fixed orientation)" 110 | 111 | if interaction_mode != InteractiveMarkerControl.NONE: 112 | control_modes_dict = { 113 | InteractiveMarkerControl.MOVE_3D : "MOVE_3D", 114 | InteractiveMarkerControl.ROTATE_3D : "ROTATE_3D", 115 | InteractiveMarkerControl.MOVE_ROTATE_3D : "MOVE_ROTATE_3D" } 116 | int_marker.name += "_" + control_modes_dict[interaction_mode] 117 | int_marker.description = "3D Control" 118 | if show_6dof: 119 | int_marker.description += " + 6-DOF controls" 120 | int_marker.description += "\n" + control_modes_dict[interaction_mode] 121 | 122 | if show_6dof: 123 | control = InteractiveMarkerControl() 124 | control.orientation.w = 1 125 | control.orientation.x = 1 126 | control.orientation.y = 0 127 | control.orientation.z = 0 128 | control.name = "rotate_x" 129 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 130 | if fixed: 131 | control.orientation_mode = InteractiveMarkerControl.FIXED 132 | int_marker.controls.append(control) 133 | 134 | control = InteractiveMarkerControl() 135 | control.orientation.w = 1 136 | control.orientation.x = 1 137 | control.orientation.y = 0 138 | control.orientation.z = 0 139 | control.name = "move_x" 140 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 141 | if fixed: 142 | control.orientation_mode = InteractiveMarkerControl.FIXED 143 | int_marker.controls.append(control) 144 | 145 | control = InteractiveMarkerControl() 146 | control.orientation.w = 1 147 | control.orientation.x = 0 148 | control.orientation.y = 1 149 | control.orientation.z = 0 150 | control.name = "rotate_z" 151 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 152 | if fixed: 153 | control.orientation_mode = InteractiveMarkerControl.FIXED 154 | int_marker.controls.append(control) 155 | 156 | control = InteractiveMarkerControl() 157 | control.orientation.w = 1 158 | control.orientation.x = 0 159 | control.orientation.y = 1 160 | control.orientation.z = 0 161 | control.name = "move_z" 162 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 163 | if fixed: 164 | control.orientation_mode = InteractiveMarkerControl.FIXED 165 | int_marker.controls.append(control) 166 | 167 | control = InteractiveMarkerControl() 168 | control.orientation.w = 1 169 | control.orientation.x = 0 170 | control.orientation.y = 0 171 | control.orientation.z = 1 172 | control.name = "rotate_y" 173 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 174 | if fixed: 175 | control.orientation_mode = InteractiveMarkerControl.FIXED 176 | int_marker.controls.append(control) 177 | 178 | control = InteractiveMarkerControl() 179 | control.orientation.w = 1 180 | control.orientation.x = 0 181 | control.orientation.y = 0 182 | control.orientation.z = 1 183 | control.name = "move_y" 184 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 185 | if fixed: 186 | control.orientation_mode = InteractiveMarkerControl.FIXED 187 | int_marker.controls.append(control) 188 | 189 | server.insert(int_marker, processFeedback) 190 | menu_handler.apply( server, int_marker.name ) 191 | 192 | 193 | if __name__=="__main__": 194 | rospy.init_node("basic_controls") 195 | 196 | server = InteractiveMarkerServer("basic_controls") 197 | 198 | menu_handler.insert( "First Entry", callback=processFeedback ) 199 | 200 | position = Point(0.5, 0, 0.21) 201 | make6DofMarker( False, InteractiveMarkerControl.NONE, position, True) 202 | 203 | server.applyChanges() 204 | 205 | rospy.spin() 206 | 207 | -------------------------------------------------------------------------------- /Kinematics/Demo/Collision_Demo.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronzguan/Franka-Robot-Path-Planning/159511d01ff8392714a0675348d83c1230ab8336/Kinematics/Demo/Collision_Demo.mp4 -------------------------------------------------------------------------------- /Kinematics/Demo/Has_Collision.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronzguan/Franka-Robot-Path-Planning/159511d01ff8392714a0675348d83c1230ab8336/Kinematics/Demo/Has_Collision.png -------------------------------------------------------------------------------- /Kinematics/Demo/No_Collision.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronzguan/Franka-Robot-Path-Planning/159511d01ff8392714a0675348d83c1230ab8336/Kinematics/Demo/No_Collision.png -------------------------------------------------------------------------------- /Kinematics/collision_boxes.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | """ 4 | Copyright (c) 2011, Willow Garage, Inc. 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above copyright 13 | notice, this list of conditions and the following disclaimer in the 14 | documentation and/or other materials provided with the distribution. 15 | * Neither the name of the Willow Garage, Inc. nor the names of its 16 | contributors may be used to endorse or promote products derived from 17 | this software without specific prior written permission. 18 | 19 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 20 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 21 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 22 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 23 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 24 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 25 | SUBSTITUTE GOODS OR SERVICES LOSS OF USE, DATA, OR PROFITS OR BUSINESS 26 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 27 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 28 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | POSSIBILITY OF SUCH DAMAGE. 30 | """ 31 | 32 | import rospy 33 | import copy 34 | 35 | from interactive_markers.interactive_marker_server import * 36 | from interactive_markers.menu_handler import * 37 | from visualization_msgs.msg import * 38 | from geometry_msgs.msg import Point, Quaternion 39 | 40 | from math import sin 41 | 42 | server = None 43 | menu_handler = MenuHandler() 44 | 45 | def processFeedback( feedback ): 46 | s = "Feedback from marker '" + feedback.marker_name 47 | s += "' / control '" + feedback.control_name + "'" 48 | 49 | mp = "" 50 | if feedback.mouse_point_valid: 51 | mp = " at " + str(feedback.mouse_point.x) 52 | mp += ", " + str(feedback.mouse_point.y) 53 | mp += ", " + str(feedback.mouse_point.z) 54 | mp += " in frame " + feedback.header.frame_id 55 | 56 | if feedback.event_type == InteractiveMarkerFeedback.BUTTON_CLICK: 57 | rospy.loginfo( s + ": button click" + mp + "." ) 58 | elif feedback.event_type == InteractiveMarkerFeedback.MENU_SELECT: 59 | rospy.loginfo( s + ": menu item " + str(feedback.menu_entry_id) + " clicked" + mp + "." ) 60 | elif feedback.event_type == InteractiveMarkerFeedback.POSE_UPDATE: 61 | rospy.loginfo( s + ": pose changed") 62 | elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_DOWN: 63 | rospy.loginfo( s + ": mouse down" + mp + "." ) 64 | elif feedback.event_type == InteractiveMarkerFeedback.MOUSE_UP: 65 | rospy.loginfo( s + ": mouse up" + mp + "." ) 66 | 67 | print(feedback.pose.position.x) 68 | print(feedback.pose.position.y) 69 | print(feedback.pose.position.z) 70 | print(feedback.pose.orientation.x) 71 | print(feedback.pose.orientation.y) 72 | print(feedback.pose.orientation.z) 73 | print(feedback.pose.orientation.w) 74 | server.applyChanges() 75 | 76 | def makeBox( msg , size): 77 | marker = Marker() 78 | 79 | marker.type = Marker.CUBE 80 | marker.scale.x = msg.scale * size[0] 81 | marker.scale.y = msg.scale * size[1] 82 | marker.scale.z = msg.scale * size[2] 83 | marker.color.r = 0.5 84 | marker.color.g = 0.5 85 | marker.color.b = 0.5 86 | marker.color.a = 1.0 87 | 88 | return marker 89 | 90 | def makeBoxControl( msg , size): 91 | control = InteractiveMarkerControl() 92 | control.always_visible = True 93 | control.markers.append( makeBox(msg, size) ) 94 | msg.controls.append( control ) 95 | return control 96 | 97 | 98 | ##################################################################### 99 | # Marker Creation 100 | 101 | def make6DofMarker( fixed, interaction_mode, position, size, name, show_6dof = False, orientation=None): 102 | int_marker = InteractiveMarker() 103 | int_marker.header.frame_id = "panda_link0" 104 | int_marker.pose.position = position 105 | if orientation is not None: 106 | int_marker.pose.orientation = orientation 107 | int_marker.scale = 1 108 | 109 | int_marker.name = name 110 | int_marker.description = "Simple 6-DOF Control" 111 | 112 | # insert a box 113 | makeBoxControl(int_marker, size) 114 | int_marker.controls[0].interaction_mode = interaction_mode 115 | 116 | if fixed: 117 | int_marker.name += "_fixed" 118 | int_marker.description += "\n(fixed orientation)" 119 | 120 | if interaction_mode != InteractiveMarkerControl.NONE: 121 | control_modes_dict = { 122 | InteractiveMarkerControl.MOVE_3D : "MOVE_3D", 123 | InteractiveMarkerControl.ROTATE_3D : "ROTATE_3D", 124 | InteractiveMarkerControl.MOVE_ROTATE_3D : "MOVE_ROTATE_3D" } 125 | int_marker.name += "_" + control_modes_dict[interaction_mode] 126 | int_marker.description = "3D Control" 127 | if show_6dof: 128 | int_marker.description += " + 6-DOF controls" 129 | int_marker.description += "\n" + control_modes_dict[interaction_mode] 130 | 131 | if show_6dof: 132 | control = InteractiveMarkerControl() 133 | control.orientation.w = 1 134 | control.orientation.x = 1 135 | control.orientation.y = 0 136 | control.orientation.z = 0 137 | control.name = "rotate_x" 138 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 139 | if fixed: 140 | control.orientation_mode = InteractiveMarkerControl.FIXED 141 | int_marker.controls.append(control) 142 | 143 | control = InteractiveMarkerControl() 144 | control.orientation.w = 1 145 | control.orientation.x = 1 146 | control.orientation.y = 0 147 | control.orientation.z = 0 148 | control.name = "move_x" 149 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 150 | if fixed: 151 | control.orientation_mode = InteractiveMarkerControl.FIXED 152 | int_marker.controls.append(control) 153 | 154 | control = InteractiveMarkerControl() 155 | control.orientation.w = 1 156 | control.orientation.x = 0 157 | control.orientation.y = 1 158 | control.orientation.z = 0 159 | control.name = "rotate_z" 160 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 161 | if fixed: 162 | control.orientation_mode = InteractiveMarkerControl.FIXED 163 | int_marker.controls.append(control) 164 | 165 | control = InteractiveMarkerControl() 166 | control.orientation.w = 1 167 | control.orientation.x = 0 168 | control.orientation.y = 1 169 | control.orientation.z = 0 170 | control.name = "move_z" 171 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 172 | if fixed: 173 | control.orientation_mode = InteractiveMarkerControl.FIXED 174 | int_marker.controls.append(control) 175 | 176 | control = InteractiveMarkerControl() 177 | control.orientation.w = 1 178 | control.orientation.x = 0 179 | control.orientation.y = 0 180 | control.orientation.z = 1 181 | control.name = "rotate_y" 182 | control.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS 183 | if fixed: 184 | control.orientation_mode = InteractiveMarkerControl.FIXED 185 | int_marker.controls.append(control) 186 | 187 | control = InteractiveMarkerControl() 188 | control.orientation.w = 1 189 | control.orientation.x = 0 190 | control.orientation.y = 0 191 | control.orientation.z = 1 192 | control.name = "move_y" 193 | control.interaction_mode = InteractiveMarkerControl.MOVE_AXIS 194 | if fixed: 195 | control.orientation_mode = InteractiveMarkerControl.FIXED 196 | int_marker.controls.append(control) 197 | 198 | server.insert(int_marker, processFeedback) 199 | menu_handler.apply( server, int_marker.name ) 200 | 201 | 202 | if __name__=="__main__": 203 | rospy.init_node("basic_controls") 204 | 205 | server = InteractiveMarkerServer("basic_controls") 206 | 207 | menu_handler.insert( "First Entry", callback=processFeedback ) 208 | 209 | position1 = Point(-0.04, 0, 0.05) 210 | size1 = [0.23, 0.2, 0.1] 211 | make6DofMarker( True, InteractiveMarkerControl.NONE, position1, size1, "box1", True) 212 | 213 | position2 = Point(-0.009, 0, 0.15) 214 | size2 = [0.13, 0.12, 0.1] 215 | make6DofMarker( True, InteractiveMarkerControl.NONE, position2, size2, "box2", True) 216 | 217 | position3 = Point(0, -0.0318194814026, 0.250960588455) 218 | size3 = [0.12, 0.1, 0.2] 219 | orientation3 = Quaternion(0.307908415794, 0, 0, 0.951416134834) 220 | make6DofMarker( False, InteractiveMarkerControl.NONE, position3, size3, "box3", True, orientation3) 221 | 222 | position4 = Point(-0.0008, 0, 0.333) 223 | size4 = [0.12, 0.27, 0.11] 224 | make6DofMarker( True, InteractiveMarkerControl.NONE, position4, size4, "box4", True) 225 | 226 | position5 = Point(0, 0.042, 0.42) 227 | size5 = [0.12, 0.1, 0.2] 228 | orientation5 = Quaternion(0.307908415794, 0, 0, 0.951416134834) 229 | make6DofMarker( False, InteractiveMarkerControl.NONE, position5, size5, "box5", True, orientation5) 230 | 231 | position6 = Point(0.00687, 0, 0.53) 232 | size6 = [0.13, 0.12, 0.22] 233 | make6DofMarker( True, InteractiveMarkerControl.NONE, position6, size6, "box6", True) 234 | 235 | position7 = Point(0.0745, 0, 0.653) 236 | size7 = [0.13, 0.23, 0.12] 237 | make6DofMarker( True, InteractiveMarkerControl.NONE, position7, size7, "box7", True) 238 | 239 | position8 = Point(0.00422, 0.00367, 0.77) 240 | size8 = [0.12, 0.12, 0.22] 241 | make6DofMarker( False, InteractiveMarkerControl.NONE, position8, size8, "box8", True) 242 | 243 | position9 = Point(0, 0.0745, 0.9077) 244 | size9 = [0.12, 0.12, 0.2] 245 | make6DofMarker( True, InteractiveMarkerControl.NONE, position9, size9, "box9", True) 246 | 247 | position10 = Point(0.00328, 0.0176, 1.0275) 248 | size10 = [0.13, 0.23, 0.12] 249 | make6DofMarker( True, InteractiveMarkerControl.NONE, position10, size10, "box10", True) 250 | 251 | position11 = Point(0.0744, -0.0092, 1.0247) 252 | size11 = [0.12, 0.12, 0.2] 253 | make6DofMarker( True, InteractiveMarkerControl.NONE, position11, size11, "box11", True) 254 | 255 | position12 = Point(0.0744, -0.0092, 0.9223) 256 | size12 = [0.10, 0.23, 0.15] 257 | orientation12 = Quaternion(0.0189154259861, -0.0100818071514, 0.470246970654, 0.882274568081) 258 | make6DofMarker( True, InteractiveMarkerControl.NONE, position12, size12, "box12", True, orientation12) 259 | 260 | server.applyChanges() 261 | 262 | rospy.spin() 263 | 264 | -------------------------------------------------------------------------------- /Kinematics/franka_robot.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | 4 | 5 | def parse_urdf(urdf_file_path): 6 | ''' 7 | TODO(Q3.1.1) 8 | 9 | Implement a urdf file parser and extract the origins and axes for each joint into numpy arrays. 10 | Arguments: string 11 | Returns: A dictionary with numpy arrays that contain the origins and axes of each joint 12 | origin = [x y z roll pitch yaw] 13 | ''' 14 | 15 | num_joints = 0 # Change this to the number of times you encounter the word "axis" in the document 16 | 17 | urdf_file = open(urdf_file_path, "r") 18 | urdf_lines = urdf_file.readlines() 19 | for line in urdf_lines: 20 | if "axis" in line: 21 | num_joints += 1 22 | 23 | origin = np.zeros((num_joints + 1, 6)) 24 | axis = np.zeros((num_joints + 1, 3)) 25 | 26 | index = 0 27 | for line in urdf_lines: 28 | if "origin" in line: 29 | p = line.split('\"') 30 | rpy_list = p[1].split(' ') 31 | xyz_list = p[3].split(' ') 32 | if len(rpy_list) > 1 and len(xyz_list) > 1: 33 | for i, (trans, rot) in enumerate(zip(xyz_list, rpy_list)): 34 | if '$' in rot: 35 | rot = rot.replace('$', '').replace('{', '').replace('}', '') 36 | nums = rot.split('/') 37 | if '-' in nums[0]: 38 | origin[index, i + 3] = -math.pi / float(nums[1]) 39 | else: 40 | origin[index, i + 3] = math.pi / float(nums[1]) 41 | else: 42 | origin[index, i + 3] = float(rot) 43 | 44 | origin[index, i] = float(trans) 45 | if "axis" in line: 46 | p = line.split('\"') 47 | axis_list = p[1].split(' ') 48 | for i, ax in enumerate(axis_list): 49 | axis[index, i] = float(ax) 50 | index += 1 51 | # Since the end-effector transformation is not included in the franka urdf, I will manually provide 52 | # the transformation here for you from the flange frame. 53 | origin[-1, 2] = 0.1034 54 | 55 | return {'origin': origin, 'axis': axis} 56 | 57 | 58 | class FrankaRobot(): 59 | 60 | def __init__(self, urdf_file_path, dh_params, num_dof): 61 | 62 | self.robot_params = parse_urdf(urdf_file_path) 63 | self.dh_params = dh_params 64 | self.num_dof = num_dof 65 | self.desc_box_list = self.get_robot_description_as_box() 66 | 67 | def forward_kinematics_urdf(self, joints): 68 | ''' 69 | TODO(Q3.1.2) 70 | 71 | Calculate the position of each joint using the robot_params 72 | Arguments: array of joint positions (rad) 73 | Returns: A numpy array that contains the 4x4 transformation matrices from the base to the position of each joint. 74 | ''' 75 | 76 | axis = self.robot_params['axis'] 77 | origin = self.robot_params['origin'] 78 | forward_kinematics = np.zeros((axis.shape[0], 4, 4)) 79 | last_T = np.identity(4) 80 | for i in range(len(origin)): 81 | x, y, z, roll, pitch, yaw = origin[i] 82 | translation_matrix = self.generate_translation_matrix(x,y,z) 83 | rotation_matrix = self.generate_rotation_matrix_from_rpy(roll, pitch, yaw) 84 | 85 | if i < len(joints): 86 | pos = joints[i] 87 | x_axis, y_axis, z_axis = axis[i] 88 | exp_map = np.asarray(([0, -z_axis, y_axis, 0], 89 | [z_axis, 0, -x_axis, 0], 90 | [-y_axis, x_axis, 0, 0], 91 | [0, 0, 0, 0])) 92 | R = np.identity(4) + math.sin(pos) * exp_map + (1-math.cos(pos))*np.dot(exp_map, exp_map) 93 | # R = np.asarray(([math.cos(pos), -math.sin(pos), 0, 0], 94 | # [math.sin(pos), math.cos(pos), 0, 0], 95 | # [0, 0, 1, 0], 96 | # [0, 0, 0, 1])) 97 | else: 98 | R = np.identity(4) 99 | 100 | T = translation_matrix.dot(rotation_matrix).dot(R) 101 | T = np.dot(last_T, T) 102 | last_T = T 103 | forward_kinematics[i] = T 104 | 105 | return forward_kinematics 106 | 107 | def forward_kinematics_dh(self, joints): 108 | ''' 109 | TODO(Q3.2.1) 110 | 111 | Calculate the position of each joint using the dh_params 112 | Arguments: array of joint positions (rad) 113 | Returns: A numpy array that contains the 4x4 transformation matrices from the base to the position of each joint. 114 | ''' 115 | 116 | forward_kinematics = np.zeros((self.dh_params.shape[0], 4, 4)) 117 | last_T = np.identity(4) 118 | for i in range(self.dh_params.shape[0]): 119 | a, d, alpha, _ = self.dh_params[i] 120 | if i < len(joints): 121 | theta = joints[i] 122 | else: 123 | theta = 0 124 | rotX = np.asarray(([1, 0, 0, 0], 125 | [0, math.cos(alpha), -math.sin(alpha), 0], 126 | [0, math.sin(alpha), math.cos(alpha), 0], 127 | [0, 0, 0, 1])) 128 | transX = np.asarray(([1, 0, 0, a], 129 | [0, 1, 0, 0], 130 | [0, 0, 1, 0], 131 | [0, 0, 0, 1])) 132 | transZ = np.asarray(([1, 0, 0, 0], 133 | [0, 1, 0, 0], 134 | [0, 0, 1, d], 135 | [0, 0, 0, 1])) 136 | rotZ = np.asarray(([math.cos(theta), -math.sin(theta), 0, 0], 137 | [math.sin(theta), math.cos(theta), 0, 0], 138 | [0, 0, 1, 0], 139 | [0, 0, 0, 1])) 140 | T = rotX.dot(transX).dot(transZ).dot(rotZ) 141 | T = np.dot(last_T, T) 142 | 143 | last_T = T 144 | forward_kinematics[i] = T 145 | return forward_kinematics 146 | 147 | def ee(self, joints): 148 | ''' 149 | TODO(Q3.2.2) 150 | 151 | Use one of your forward kinematics implementations to return the position of the end-effector. 152 | Arguments: array of joint positions (rad) 153 | Returns: A numpy array that contains the [x, y, z, roll, pitch, yaw] location of the end-effector. 154 | ''' 155 | dh_fk = self.forward_kinematics_dh(joints) 156 | H = dh_fk[-1] 157 | x, y, z, roll, pitch, yaw = self.decompose_homogeneous_matrx(H) 158 | return np.array([x, y, z, roll, pitch, yaw]) 159 | 160 | def jacobian(self, joints): 161 | ''' 162 | TODO(Q4.1.1) 163 | 164 | Calculate the end-effector jacobian analytically using your forward kinematics 165 | Arguments: array of joint positions (rad) 166 | Returns: A numpy array that contains the 6 x num_dof end-effector jacobian. 167 | ''' 168 | jacobian = np.zeros((6, self.num_dof)) 169 | dh_fk = self.forward_kinematics_dh(joints) 170 | for i in range(self.num_dof): 171 | offset = dh_fk[-1][:-1, -1] - dh_fk[i][:-1, -1] 172 | angular_v = np.dot(dh_fk[i][:-1, :-1], [0, 0, 1]) 173 | linear_v = np.cross(angular_v, offset) 174 | jacobian[:, i] = np.concatenate((linear_v, angular_v), axis=0) 175 | return jacobian 176 | 177 | def inverse_kinematics(self, desired_ee_pos, current_joints): 178 | ''' 179 | TODO(Q5.1.1) 180 | 181 | Implement inverse kinematics using one of the methods mentioned in class. 182 | Arguments: desired_ee_pos which is a np array of [x, y, z, r, p, y] which represent the desired end-effector position of the robot 183 | current_joints which represents the current location of the robot 184 | Returns: A numpy array that contains the joints required in order to achieve the desired end-effector position. 185 | ''' 186 | joints = current_joints[:self.num_dof] 187 | current_ee_pos = self.ee(joints) 188 | ee_error = desired_ee_pos - current_ee_pos 189 | while np.linalg.norm(ee_error) > 1e-10: 190 | J = self.jacobian(joints) 191 | ## Jacobian Transpose Approach 192 | joints_error = 0.2 * np.dot(J.transpose(), ee_error) 193 | ## Pseudo-Inverse Approach, cannot work in this case 194 | # joints_error = np.dot(np.transpose(J), np.linalg.inv(np.dot(J, np.transpose(J)))).dot(ee_error) 195 | joints += joints_error 196 | current_ee_pos = self.ee(joints) 197 | ee_error = desired_ee_pos - current_ee_pos 198 | return joints 199 | 200 | def check_box_collision(self, joints, box): 201 | ''' 202 | TODO(Q6.1.1) 203 | 204 | Implement collision checking with a box. 205 | Arguments: joints represents the current location of the robot 206 | box contains the position of the center of the box [x, y, z, r, p, y] and the length, width, and height [l, w, h] 207 | Returns: A boolean where True means the box is in collision with the arm and false means that there are no collisions. 208 | ''' 209 | x_box, y_box, z_box, roll_box, pitch_box, yaw_box, l_box, w_box, h_box = box 210 | rotation_matrix_box = self.generate_rotation_matrix_from_rpy(roll_box, pitch_box, yaw_box) 211 | Bx, By, Bz = self.get_local_axis(rotation_matrix_box) 212 | Bl, Bw, Bh = l_box / 2, w_box / 2, h_box / 2 213 | 214 | # Get the homogeneous matrix for each joint by using forward kinematics 215 | dh_fk = self.forward_kinematics_dh(joints) 216 | 217 | num_boxes = len(self.desc_box_list) 218 | in_collision = np.ones(num_boxes) 219 | 220 | # Interating over boxes to check if each of the box has collision with the input box 221 | for i, desc_box in enumerate(self.desc_box_list): 222 | x, y, z = desc_box["trans"] 223 | qw, qx, qy, qz = desc_box["rot"] 224 | l, w, h = desc_box["size"] 225 | 226 | # Get the translation and rotation matrix for local transformation 227 | translation_matrix = self.generate_translation_matrix(x, y, z) 228 | rotation_matrix = self.generate_rotation_matrix_from_quaternion(qw, qx, qy, qz) 229 | # Get the local transformation matrix 230 | T = np.dot(translation_matrix, rotation_matrix) 231 | # Get the homogeneous matrix for each description box referred to the base using forward kinematics 232 | H = np.dot(dh_fk[desc_box["link"] - 1], T) 233 | # Decompose the homogeneous matrix to get the current position and orientation referred to the base 234 | x, y, z, roll, pitch, yaw = self.decompose_homogeneous_matrx(H) 235 | 236 | Ax, Ay, Az = self.get_local_axis(rotation_matrix) 237 | Al, Aw, Ah = l / 2, w / 2, h / 2 238 | 239 | # Using seperating axis theorem for collision checking 240 | proj_axis = [Ax, Ay, Az, Bx, By, Bz, np.cross(Ax, Bx), np.cross(Ax, By), np.cross(Ax, Bz), np.cross(Ay, Bx), 241 | np.cross(Ay, By), np.cross(Ay, Bz), np.cross(Az, Bx), np.cross(Az, By), np.cross(Az, Bz)] 242 | 243 | for axis in proj_axis: 244 | proj_origin_dist = abs(np.dot(np.asarray([x_box, y_box, z_box]) - np.asarray([x, y, z]), axis)) 245 | 246 | proj_box_length = abs(np.dot(Al * Ax, axis)) + abs(np.dot(Aw * Ay, axis)) \ 247 | + abs(np.dot(Ah * Az, axis)) + abs(np.dot(Bl * Bx, axis)) + \ 248 | abs(np.dot(Bw * By, axis)) + abs(np.dot(Bh * Bz, axis)) 249 | 250 | if proj_origin_dist > proj_box_length: 251 | in_collision[i] = 0 # No collision for the i-th box 252 | break 253 | 254 | if sum(in_collision) == 0: 255 | # All boxes have no collision 256 | return False 257 | else: 258 | print('The Box {} has collision'.format(*list(np.where(in_collision == 1)[0] + 1))) 259 | return True 260 | 261 | def get_robot_description_as_box(self): 262 | """ 263 | :return A list of description boxes for collision checking 264 | """ 265 | box1 = {"link": 1, "trans": np.asarray([-0.04, 0, - 0.283]), "rot": np.asarray([1, 0, 0, 0]), "size": np.asarray([0.23, 0.2, 0.1])} 266 | box2 = {"link": 1, "trans": np.asarray([-0.009, 0, -0.183]), "rot": np.asarray([1, 0, 0, 0]), "size": np.asarray([0.13, 0.12, 0.1])} 267 | box3 = {"link": 1, "trans": np.asarray([0, -0.032, -0.082]), "rot": np.asarray([0.9514, 0.3079, 0, 0]), "size": np.asarray([0.12, 0.1, 0.2])} 268 | box4 = {"link": 1, "trans": np.asarray([-0.008, 0, 0]), "rot": np.asarray([1, 0, 0, 0]), "size": np.asarray([0.15, 0.27, 0.11])} 269 | box5 = {"link": 1, "trans": np.asarray([0, 0.042, 0.067]), "rot": np.asarray([0.9514, 0.3079, 0, 0]), "size": np.asarray([0.12, 0.1, 0.2])} 270 | box6 = {"link": 3, "trans": np.asarray([0.00687, 0, -0.139]), "rot": np.asarray([1, 0, 0, 0]), "size": np.asarray([0.13, 0.12, 0.25])} 271 | box7 = {"link": 4, "trans": np.asarray([-0.008, 0.004, 0]), "rot": np.asarray([0.7071, -0.7071, 0, 0]), "size": np.asarray([0.13, 0.23, 0.15])} 272 | box8 = {"link": 5, "trans": np.asarray([0.00422, 0.05367, -0.121]), "rot": np.asarray([0.9962, -0.08715, 0, 0]), "size": np.asarray([0.12, 0.12, 0.4])} 273 | box9 = {"link": 5, "trans": np.asarray([0.00422, 0.00367, -0.263]), "rot": np.asarray([1, 0, 0, 0]), "size": np.asarray([0.12, 0.12, 0.25])} 274 | box10 = {"link": 5, "trans": np.asarray([0.00328, 0.0176, -0.0055]), "rot": np.asarray([1, 0, 0, 0]), "size": np.asarray([0.13, 0.23, 0.12])} 275 | box11 = {"link": 7, "trans": np.asarray([-0.0136, 0.0092, 0.0083]), "rot": np.asarray([0, 1, 0, 0]), "size": np.asarray([0.12, 0.12, 0.2])} 276 | box12 = {"link": 7, "trans": np.asarray([-0.0136, 0.0092, 0.1407]), "rot": np.asarray([0.9239, 0, 0, -0.3827]), "size": np.asarray([0.08, 0.22, 0.17])} 277 | 278 | return [box1, box2, box3, box4, box5, box6, box7, box8, box9, box10, box11, box12] 279 | 280 | def generate_translation_matrix(self, x, y, z): 281 | translation_matrix = np.asarray(([1, 0, 0, x], 282 | [0, 1, 0, y], 283 | [0, 0, 1, z], 284 | [0, 0, 0, 1])) 285 | return translation_matrix 286 | 287 | def generate_rotation_matrix_from_quaternion(self, qw, qx, qy, qz): 288 | rotation_matrix = np.asarray( 289 | ([1 - 2 * (qy ** 2) - 2 * (qz ** 2), 2 * qx * qy - 2 * qz * qw, 2 * qx * qz + 2 * qy * qw, 0], 290 | [2 * qx * qy + 2 * qz * qw, 1 - 2 * (qx ** 2) - 2 * (qz ** 2), 2 * qy * qz - 2 * qx * qw, 0], 291 | [2 * qx * qz - 2 * qy * qw, 2 * qy * qz + 2 * qx * qw, 1 - 2 * (qx ** 2) - 2 * (qy ** 2), 0], 292 | [0, 0, 0, 1])) 293 | return rotation_matrix 294 | 295 | def generate_rotation_matrix_from_rpy(self, roll, pitch, yaw): 296 | """ 297 | :param roll: roll angle of the rotation matrix in rad 298 | :param pitch: pitch angle of the rotation matrix in rad 299 | :param yaw: yaw angle of the rotation matrix in rad 300 | :return: A 4x4 rotation matrix 301 | """ 302 | rotX = np.asarray(([1, 0, 0, 0], 303 | [0, math.cos(roll), -math.sin(roll), 0], 304 | [0, math.sin(roll), math.cos(roll), 0], 305 | [0, 0, 0, 1])) 306 | rotY = np.asarray(([math.cos(pitch), 0, math.sin(pitch), 0], 307 | [0, 1, 0, 0], 308 | [-math.sin(pitch), 0, math.cos(pitch), 0], 309 | [0, 0, 0, 1])) 310 | rotZ = np.asarray(([math.cos(yaw), -math.sin(yaw), 0, 0], 311 | [math.sin(yaw), math.cos(yaw), 0, 0], 312 | [0, 0, 1, 0], 313 | [0, 0, 0, 1])) 314 | 315 | return rotX.dot(rotY).dot(rotZ) 316 | 317 | def decompose_homogeneous_matrx(self, matrix): 318 | """ 319 | :param matrix: A 4x4 homogeneous matrix related to the base of the robot 320 | :return: [x y z roll pitch yaw] of the current joint related to the base of the robot 321 | """ 322 | x, y, z = matrix[:-1, -1] 323 | R = matrix[:-1, :-1] 324 | 325 | # roll = math.atan2(R[2, 1], R[2, 2]) 326 | # pitch = math.atan2(-R[2, 0], (math.sqrt(R[2, 1] ** 2 + R[2, 2] ** 2))) 327 | # yaw = math.atan2(R[1, 0], R[0, 0]) 328 | 329 | sy = math.sqrt(R[2, 1]**2 + R[2, 2]**2) 330 | singular = sy < 1e-6 331 | if not singular: 332 | if R[2, 2] < 1e-6: 333 | roll = math.pi / 2 334 | else: 335 | roll = math.atan2(R[2, 1], R[2, 2]) 336 | if R[0, 0] < 1e-6: 337 | yaw = math.pi / 2 338 | else: 339 | yaw = math.atan2(R[1, 0], R[0, 0]) 340 | pitch = math.atan2(-R[2, 0], sy) 341 | else: 342 | roll = math.atan2(R[2, 1], R[2, 2]) 343 | pitch = math.pi / 2 344 | yaw = math.atan2(R[1, 0], R[0, 0]) 345 | 346 | return np.array([x, y, z, roll, pitch, yaw]) 347 | 348 | def get_local_axis(self, rotation_matrix): 349 | """ 350 | :param rotation_matrix: 4 x 4 homogeneous matrix 351 | :return: local x-axis, y-axis, and z-axis, unit vector, not homogeneous 352 | """ 353 | x = rotation_matrix[:-1, 0] 354 | y = rotation_matrix[:-1, 1] 355 | z = rotation_matrix[:-1, 2] 356 | 357 | x = x / np.linalg.norm(x) 358 | y = y / np.linalg.norm(y) 359 | z = z / np.linalg.norm(z) 360 | 361 | return x, y, z 362 | -------------------------------------------------------------------------------- /Kinematics/franka_robot.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | -------------------------------------------------------------------------------- /Kinematics/franka_visualization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 89 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /InteractiveMarkers1 10 | - /Marker1 11 | Splitter Ratio: 0.5 12 | Tree Height: 717 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.5886790156364441 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Preferences: 33 | PromptSaveOnExit: true 34 | Toolbars: 35 | toolButtonStyle: 2 36 | Visualization Manager: 37 | Class: "" 38 | Displays: 39 | - Alpha: 0.5 40 | Cell Size: 1 41 | Class: rviz/Grid 42 | Color: 160; 160; 164 43 | Enabled: true 44 | Line Style: 45 | Line Width: 0.029999999329447746 46 | Value: Lines 47 | Name: Grid 48 | Normal Cell Count: 0 49 | Offset: 50 | X: 0 51 | Y: 0 52 | Z: 0 53 | Plane: XY 54 | Plane Cell Count: 10 55 | Reference Frame: 56 | Value: true 57 | - Alpha: 1 58 | Class: rviz/RobotModel 59 | Collision Enabled: false 60 | Enabled: true 61 | Links: 62 | All Links Enabled: true 63 | Expand Joint Details: false 64 | Expand Link Details: false 65 | Expand Tree: false 66 | Link Tree Style: Links in Alphabetic Order 67 | panda_hand: 68 | Alpha: 1 69 | Show Axes: false 70 | Show Trail: false 71 | Value: true 72 | panda_leftfinger: 73 | Alpha: 1 74 | Show Axes: false 75 | Show Trail: false 76 | Value: true 77 | panda_link0: 78 | Alpha: 1 79 | Show Axes: false 80 | Show Trail: false 81 | Value: true 82 | panda_link1: 83 | Alpha: 1 84 | Show Axes: false 85 | Show Trail: false 86 | Value: true 87 | panda_link2: 88 | Alpha: 1 89 | Show Axes: false 90 | Show Trail: false 91 | Value: true 92 | panda_link3: 93 | Alpha: 1 94 | Show Axes: false 95 | Show Trail: false 96 | Value: true 97 | panda_link4: 98 | Alpha: 1 99 | Show Axes: false 100 | Show Trail: false 101 | Value: true 102 | panda_link5: 103 | Alpha: 1 104 | Show Axes: false 105 | Show Trail: false 106 | Value: true 107 | panda_link6: 108 | Alpha: 1 109 | Show Axes: false 110 | Show Trail: false 111 | Value: true 112 | panda_link7: 113 | Alpha: 1 114 | Show Axes: false 115 | Show Trail: false 116 | Value: true 117 | panda_link8: 118 | Alpha: 1 119 | Show Axes: false 120 | Show Trail: false 121 | panda_rightfinger: 122 | Alpha: 1 123 | Show Axes: false 124 | Show Trail: false 125 | Value: true 126 | Name: RobotModel 127 | Robot Description: robot_description 128 | TF Prefix: "" 129 | Update Interval: 0 130 | Value: true 131 | Visual Enabled: true 132 | - Class: rviz/InteractiveMarkers 133 | Enable Transparency: true 134 | Enabled: true 135 | Name: InteractiveMarkers 136 | Show Axes: false 137 | Show Descriptions: true 138 | Show Visual Aids: false 139 | Update Topic: /basic_controls/update 140 | Value: true 141 | - Class: rviz/Marker 142 | Enabled: true 143 | Marker Topic: visualization_marker 144 | Name: Marker 145 | Namespaces: 146 | {} 147 | Queue Size: 100 148 | Value: true 149 | Enabled: true 150 | Global Options: 151 | Background Color: 48; 48; 48 152 | Default Light: true 153 | Fixed Frame: panda_link0 154 | Frame Rate: 30 155 | Name: root 156 | Tools: 157 | - Class: rviz/Interact 158 | Hide Inactive Objects: true 159 | - Class: rviz/MoveCamera 160 | - Class: rviz/Select 161 | - Class: rviz/FocusCamera 162 | - Class: rviz/Measure 163 | - Class: rviz/SetInitialPose 164 | Theta std deviation: 0.2617993950843811 165 | Topic: /initialpose 166 | X std deviation: 0.5 167 | Y std deviation: 0.5 168 | - Class: rviz/SetGoal 169 | Topic: /move_base_simple/goal 170 | - Class: rviz/PublishPoint 171 | Single click: true 172 | Topic: /clicked_point 173 | Value: true 174 | Views: 175 | Current: 176 | Class: rviz/Orbit 177 | Distance: 3.393627643585205 178 | Enable Stereo Rendering: 179 | Stereo Eye Separation: 0.05999999865889549 180 | Stereo Focal Distance: 1 181 | Swap Stereo Eyes: false 182 | Value: false 183 | Focal Point: 184 | X: -0.13906224071979523 185 | Y: 0.15141408145427704 186 | Z: 0.043728869408369064 187 | Focal Shape Fixed Size: true 188 | Focal Shape Size: 0.05000000074505806 189 | Invert Z Axis: false 190 | Name: Current View 191 | Near Clip Distance: 0.009999999776482582 192 | Pitch: 0.48539769649505615 193 | Target Frame: 194 | Value: Orbit (rviz) 195 | Yaw: 0.5003982186317444 196 | Saved: ~ 197 | Window Geometry: 198 | Displays: 199 | collapsed: false 200 | Height: 1025 201 | Hide Left Dock: false 202 | Hide Right Dock: false 203 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000363fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000363000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000073d0000003efc0100000002fb0000000800540069006d006501000000000000073d000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000004b80000036300000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 204 | Selection: 205 | collapsed: false 206 | Time: 207 | collapsed: false 208 | Tool Properties: 209 | collapsed: false 210 | Views: 211 | collapsed: false 212 | Width: 1853 213 | X: 67 214 | Y: 27 215 | -------------------------------------------------------------------------------- /Kinematics/joint_trajectory.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronzguan/Franka-Robot-Path-Planning/159511d01ff8392714a0675348d83c1230ab8336/Kinematics/joint_trajectory.npy -------------------------------------------------------------------------------- /Kinematics/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | rospkg -------------------------------------------------------------------------------- /Kinematics/test_collision_boxes.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import math 3 | from sensor_msgs.msg import JointState 4 | from franka_robot import FrankaRobot 5 | 6 | def robot_state_publisher(): 7 | joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10) 8 | 9 | rospy.init_node('joint_state_publisher') 10 | rate = rospy.Rate(30) # 30 hz 11 | joint_state = JointState() 12 | joint_state.name = ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_finger_joint1', 'panda_finger_joint2'] 13 | joint_state.position = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 14 | 15 | 16 | while not rospy.is_shutdown(): 17 | joint_state.header.stamp = rospy.Time.now() 18 | joint_state_pub.publish(joint_state) 19 | rate.sleep() 20 | if __name__ == '__main__': 21 | robot_state_publisher() -------------------------------------------------------------------------------- /Kinematics/test_collision_detection.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import math 3 | import numpy as np 4 | import tf 5 | from sensor_msgs.msg import JointState 6 | from visualization_msgs.msg import Marker, InteractiveMarkerFeedback 7 | from franka_robot import FrankaRobot 8 | import os 9 | 10 | 11 | def lin_interp(start, goal, num_samples): 12 | interp = np.empty((num_samples, 0)) 13 | 14 | for i in range(np.array(start).shape[0]): 15 | interp = np.concatenate([interp, np.linspace(start[i], goal[i], num_samples).reshape((-1, 1))], axis=1) 16 | 17 | return interp 18 | 19 | 20 | class RobotStatePublisher(): 21 | 22 | def __init__(self): 23 | self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10) 24 | self.marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=10) 25 | self.box_sub = rospy.Subscriber('/basic_controls/feedback', InteractiveMarkerFeedback, self.box_callback) 26 | 27 | rospy.init_node('joint_state_publisher') 28 | self.hz = 10 29 | self.rate = rospy.Rate(self.hz) # 30 hz 30 | self.joint_state = JointState() 31 | self.joint_state.name = ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 32 | 'panda_joint6', 'panda_joint7', 'panda_finger_joint1', 'panda_finger_joint2'] 33 | self.joint_state.position = [0.0, -math.pi / 4, 0.0, -3 * math.pi / 4, 0.0, math.pi / 2, math.pi / 4, 0.0, 0.0] 34 | 35 | self.red_cylinder = Marker() 36 | self.red_cylinder.header.frame_id = "panda_link0" 37 | self.red_cylinder.type = Marker.CYLINDER 38 | self.red_cylinder.pose.position.x = 0 39 | self.red_cylinder.pose.position.y = 0.3 40 | self.red_cylinder.pose.position.z = 0 41 | self.red_cylinder.color.a = 1.0 42 | self.red_cylinder.color.r = 1.0 43 | self.red_cylinder.color.g = 0.0 44 | self.red_cylinder.color.b = 0.0 45 | self.red_cylinder.scale.x = 0.1 46 | self.red_cylinder.scale.y = 0.1 47 | self.red_cylinder.scale.z = 0.1 48 | 49 | self.green_cylinder = Marker() 50 | self.green_cylinder.header.frame_id = "panda_link0" 51 | self.green_cylinder.type = Marker.CYLINDER 52 | self.green_cylinder.pose.position.x = 0 53 | self.green_cylinder.pose.position.y = 0.3 54 | self.green_cylinder.pose.position.z = 0 55 | self.green_cylinder.color.a = 1.0 56 | self.green_cylinder.color.r = 0.0 57 | self.green_cylinder.color.g = 1.0 58 | self.green_cylinder.color.b = 0.0 59 | self.green_cylinder.scale.x = 0.1 60 | self.green_cylinder.scale.y = 0.1 61 | self.green_cylinder.scale.z = 0.1 62 | 63 | self.dh_params = np.array([[0, 0.333, 0, 0], 64 | [0, 0, -math.pi / 2, 0], 65 | [0, 0.316, math.pi / 2, 0], 66 | [0.0825, 0, math.pi / 2, 0], 67 | [-0.0825, 0.384, -math.pi / 2, 0], 68 | [0, 0, math.pi / 2, 0], 69 | [0.088, 0, math.pi / 2, 0], 70 | [0, 0.107, 0, 0], 71 | [0, 0.1034, 0, 0]]) 72 | self.fr = FrankaRobot('franka_robot.urdf', self.dh_params, 7) 73 | 74 | self.box = [0.5, 0, 0.21, 0, 0, 0, 0.15, 0.09, 0.126] 75 | 76 | self.ee_trajectory = np.empty((0, 6)) 77 | self.joint_trajectory = np.empty((0, 7)) 78 | self.joint_trajectory_index = 0 79 | self.num_joint_trajectory_points = 0 80 | 81 | if os.path.exists('joint_trajectory.npy'): 82 | self.joint_trajectory = np.load('joint_trajectory.npy') 83 | self.num_joint_trajectory_points = self.joint_trajectory.shape[0] 84 | print('Load joint_trajectory.npy as the joint_trajectory') 85 | else: 86 | self.calculate_joint_trajectory() 87 | np.save('joint_trajectory.npy', self.joint_trajectory) 88 | 89 | def calculate_joint_trajectory(self): 90 | current_joints = self.joint_state.position[:7] 91 | current_ee_pose = self.fr.ee(current_joints) 92 | desired_goal_pose_1 = np.copy(current_ee_pose) 93 | desired_goal_pose_1[0] += 0.1 94 | desired_goal_pose_1[1] += 0.1 95 | desired_goal_pose_2 = np.copy(current_ee_pose) 96 | desired_goal_pose_2[2] += 0.1 97 | desired_goal_pose_2[5] += math.pi / 2 98 | 99 | ee_trajectory_1 = lin_interp(current_ee_pose, desired_goal_pose_1, 3 * self.hz) 100 | ee_trajectory_2 = lin_interp(desired_goal_pose_1, desired_goal_pose_2, 3 * self.hz) 101 | ee_trajectory_3 = lin_interp(desired_goal_pose_2, current_ee_pose, 3 * self.hz) 102 | self.ee_trajectory = np.concatenate([ee_trajectory_1, ee_trajectory_2, ee_trajectory_3], axis=0) 103 | 104 | self.num_joint_trajectory_points = self.ee_trajectory.shape[0] 105 | self.joint_trajectory = np.zeros((self.num_joint_trajectory_points, 7)) 106 | for i in range(self.num_joint_trajectory_points): 107 | print(i) 108 | self.joint_trajectory[i, :] = self.fr.inverse_kinematics(self.ee_trajectory[i, :], current_joints) 109 | current_joints = self.joint_trajectory[i, :] 110 | 111 | def box_callback(self, data): 112 | self.box[0] = data.pose.position.x 113 | self.box[1] = data.pose.position.y 114 | self.box[2] = data.pose.position.z 115 | 116 | euler = tf.transformations.euler_from_quaternion( 117 | [data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w]) 118 | 119 | self.box[3] = euler[0] 120 | self.box[4] = euler[1] 121 | self.box[5] = euler[2] 122 | 123 | def run(self): 124 | self.joint_state.position[:7] = self.joint_trajectory[self.joint_trajectory_index, :] 125 | self.joint_trajectory_index = (self.joint_trajectory_index + 1) % self.num_joint_trajectory_points 126 | 127 | if (self.fr.check_box_collision(self.joint_state.position, self.box)): 128 | self.red_cylinder.header.stamp = rospy.Time.now() 129 | self.marker_pub.publish(self.red_cylinder) 130 | else: 131 | self.green_cylinder.header.stamp = rospy.Time.now() 132 | self.marker_pub.publish(self.green_cylinder) 133 | 134 | self.joint_state.header.stamp = rospy.Time.now() 135 | self.joint_state_pub.publish(self.joint_state) 136 | 137 | self.rate.sleep() 138 | 139 | 140 | if __name__ == '__main__': 141 | robot_state_publisher = RobotStatePublisher() 142 | 143 | while not rospy.is_shutdown(): 144 | robot_state_publisher.run() -------------------------------------------------------------------------------- /Kinematics/test_forward_kinematics.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | from franka_robot import FrankaRobot 4 | 5 | if __name__ == '__main__': 6 | dh_params = np.array([[0, 0.333, 0, 0], 7 | [0, 0, -math.pi/2, 0], 8 | [0, 0.316, math.pi/2, 0], 9 | [0.0825, 0, math.pi/2, 0], 10 | [-0.0825, 0.384, -math.pi/2, 0], 11 | [0, 0, math.pi/2, 0], 12 | [0.088, 0, math.pi/2, 0], 13 | [0, 0.107, 0, 0], 14 | [0, 0.1034, 0, 0]]) 15 | fr = FrankaRobot('franka_robot.urdf', dh_params, 7) 16 | joints = [0, -math.pi/4, 0, -3*math.pi/4, 0, math.pi/2, math.pi/4] 17 | urdf_fk = fr.forward_kinematics_urdf(joints) 18 | dh_fk = fr.forward_kinematics_dh(joints) 19 | print(np.allclose(urdf_fk, dh_fk)) 20 | -------------------------------------------------------------------------------- /Kinematics/test_inverse_kinematics.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | from franka_robot import FrankaRobot 4 | 5 | if __name__ == '__main__': 6 | dh_params = np.array([[0, 0.333, 0, 0], 7 | [0, 0, -math.pi/2, 0], 8 | [0, 0.316, math.pi/2, 0], 9 | [0.0825, 0, math.pi/2, 0], 10 | [-0.0825, 0.384, -math.pi/2, 0], 11 | [0, 0, math.pi/2, 0], 12 | [0.088, 0, math.pi/2, 0], 13 | [0, 0.107, 0, 0], 14 | [0, 0.1034, 0, 0]]) 15 | fr = FrankaRobot('franka_robot.urdf', dh_params, 7) 16 | joints = [0, -math.pi/4, 0, -3*math.pi/4, 0, math.pi/2, math.pi/4] 17 | ee = fr.ee(joints) 18 | ee[1] += 0.01 19 | new_joints = fr.inverse_kinematics(ee, joints) 20 | new_ee = fr.ee(new_joints) 21 | 22 | print(np.allclose(new_ee,ee)) -------------------------------------------------------------------------------- /Kinematics/test_jacobian.py: -------------------------------------------------------------------------------- 1 | import math 2 | import numpy as np 3 | from franka_robot import FrankaRobot 4 | 5 | if __name__ == '__main__': 6 | dh_params = np.array([[0, 0.333, 0, 0], 7 | [0, 0, -math.pi/2, 0], 8 | [0, 0.316, math.pi/2, 0], 9 | [0.0825, 0, math.pi/2, 0], 10 | [-0.0825, 0.384, -math.pi/2, 0], 11 | [0, 0, math.pi/2, 0], 12 | [0.088, 0, math.pi/2, 0], 13 | [0, 0.107, 0, 0], 14 | [0, 0.1034, 0, 0]]) 15 | fr = FrankaRobot('franka_robot.urdf', dh_params, 7) 16 | joints = np.array([0, -math.pi/4, 0.0, -3*math.pi/4, 0.0, math.pi/2, math.pi/4]) 17 | jacobian = fr.jacobian(joints) 18 | 19 | sample_jacobian = np.array([[0, 0.153882052, 0, 0.1279, 0, 0.2104, 0], 20 | [0.306890567, 0, 0.325815443, 0, 0.2104, 0, 0], 21 | [0, -0.306890567, 0, 0.472, 0, 0.088, 0], 22 | [0, 0, -0.707106781, 0, 1, 0, 0], 23 | [0, 1, 0, -1, 0, -1, 0], 24 | [1, 0, 0.707106781, 0, 0, 0, -1]]) 25 | 26 | print(np.allclose(jacobian, sample_jacobian)) -------------------------------------------------------------------------------- /Kinematics/visualize.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /Kinematics/writeup/Kinematics_Pandas.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronzguan/Franka-Robot-Path-Planning/159511d01ff8392714a0675348d83c1230ab8336/Kinematics/writeup/Kinematics_Pandas.pdf -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Aaron Z GUAN 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /Path Planning/README.md: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronzguan/Franka-Robot-Path-Planning/159511d01ff8392714a0675348d83c1230ab8336/Path Planning/README.md -------------------------------------------------------------------------------- /Path Planning/collision_boxes_publisher.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | from visualization_msgs.msg import Marker, MarkerArray 3 | import quaternion 4 | 5 | 6 | class CollisionBoxesPublisher: 7 | 8 | def __init__(self, name): 9 | self._boxes_pub = rospy.Publisher(name, MarkerArray, queue_size=10) 10 | 11 | def publish_boxes(self, boxes): 12 | markers = [] 13 | for i, box in enumerate(boxes): 14 | marker = Marker() 15 | marker.type = Marker.CUBE 16 | marker.header.stamp = rospy.Time.now() 17 | marker.header.frame_id = 'panda_link0' 18 | marker.id = i 19 | 20 | marker.lifetime = rospy.Duration() 21 | 22 | marker.pose.position.x = box[0] 23 | marker.pose.position.y = box[1] 24 | marker.pose.position.z = box[2] 25 | 26 | marker.scale.x = box[-3] 27 | marker.scale.y = box[-2] 28 | marker.scale.z = box[-1] 29 | 30 | if len(box) == 9: 31 | q = quaternion.from_euler_angles(box[3], box[4], box[5]) 32 | for k in 'wxyz': 33 | setattr(marker.pose.orientation, k, getattr(q, k)) 34 | elif len(box) == 10: 35 | for j, k in enumerate('wxyz'): 36 | setattr(marker.pose.orientation, k, box[3 + j]) 37 | else: 38 | raise ValueError('Invalid format for box!') 39 | 40 | marker.color.r = 0.5 41 | marker.color.g = 0.5 42 | marker.color.b = 0.5 43 | marker.color.a = 0.6 44 | 45 | markers.append(marker) 46 | 47 | marker_array = MarkerArray(markers) 48 | self._boxes_pub.publish(marker_array) 49 | -------------------------------------------------------------------------------- /Path Planning/demo/ConstrainedRRT.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronzguan/Franka-Robot-Path-Planning/159511d01ff8392714a0675348d83c1230ab8336/Path Planning/demo/ConstrainedRRT.mp4 -------------------------------------------------------------------------------- /Path Planning/demo/RRT.mp4: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronzguan/Franka-Robot-Path-Planning/159511d01ff8392714a0675348d83c1230ab8336/Path Planning/demo/RRT.mp4 -------------------------------------------------------------------------------- /Path Planning/franka_robot.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import quaternion 3 | from itertools import product 4 | 5 | import rospy 6 | from sensor_msgs.msg import JointState 7 | from collision_boxes_publisher import CollisionBoxesPublisher 8 | 9 | 10 | class FrankaRobot: 11 | 12 | joint_limits_low = np.array([-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973]) 13 | joint_limits_high = np.array([2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973]) 14 | home_joints = np.array([0, -np.pi / 4, 0, -3 * np.pi / 4, 0, np.pi / 2, np.pi / 4]) 15 | dh_params = np.array([[0, 0.333, 0, 0], 16 | [0, 0, -np.pi/2, 0], 17 | [0, 0.316, np.pi/2, 0], 18 | [0.0825, 0, np.pi/2, 0], 19 | [-0.0825, 0.384, -np.pi/2, 0], 20 | [0, 0, np.pi/2, 0], 21 | [0.088, 0, np.pi/2, 0], 22 | [0, 0.107, 0, 0], 23 | [0, 0.1034, 0, 0]]) 24 | num_dof = 7 25 | 26 | _dh_alpha_rot = np.array([ 27 | [1, 0, 0, 0], 28 | [0, -1, -1, 0], 29 | [0, -1, -1, 0], 30 | [0, 0, 0, 1] 31 | ], dtype=np.float32) 32 | _dh_a_trans = np.array([ 33 | [1, 0, 0, -1], 34 | [0, 1, 0, 0], 35 | [0, 0, 1, 0], 36 | [0, 0, 0, 1] 37 | ], dtype=np.float32) 38 | _dh_d_trans = np.array([ 39 | [1, 0, 0, 0], 40 | [0, 1, 0, 0], 41 | [0, 0, 1, -1], 42 | [0, 0, 0, 1] 43 | ], dtype=np.float32) 44 | _dh_theta_rot = np.array([ 45 | [-1, -1, 0, 0], 46 | [-1, -1, 0, 0], 47 | [0, 0, 1, 0], 48 | [0, 0, 0, 1] 49 | ], dtype=np.float32) 50 | 51 | collision_box_shapes = np.array([ 52 | [0.23, 0.2, 0.1], 53 | [0.13, 0.12, 0.1], 54 | [0.12, 0.1, 0.2], 55 | [0.15, 0.27, 0.11], 56 | [0.12, 0.1, 0.2], 57 | [0.13, 0.12, 0.25], 58 | [0.13, 0.23, 0.15], 59 | [0.12, 0.12, 0.4], 60 | [0.12, 0.12, 0.25], 61 | [0.13, 0.23, 0.12], 62 | [0.12, 0.12, 0.2], 63 | [0.08, 0.22, 0.17] 64 | ]) 65 | _collision_box_links = [1, 1, 1, 1, 1, 3, 4, 5, 5, 5, 7, 7] 66 | _collision_box_poses_raw = np.array([ 67 | [-.04, 0, -0.283, 1, 0, 0, 0], 68 | [-0.009, 0, -0.183, 1, 0, 0, 0], 69 | [0, -0.032, -0.082, 0.95141601, 0.30790838, 0, 0], 70 | [-0.008, 0, 0, 1, 0, 0, 0], 71 | [0, .042, .067, 0.95141601, 0.30790838, 0, 0], 72 | [0.00687, 0, -0.139, 1, 0, 0, 0], 73 | [-0.008, 0.004, 0, 0.70710678, -0.70710678, 0, 0], 74 | [0.00422, 0.05367, -0.121, 0.9961947, -0.08715574, 0, 0], 75 | [0.00422, 0.00367, -0.263, 1, 0, 0, 0], 76 | [0.00328, 0.0176, -0.0055, 1, 0, 0, 0], 77 | [-0.0136, 0.0092, 0.0083, 0, 1, 0, 0], 78 | [0.0136, -0.0092, 0.1457, 0.92387953, 0, 0, -0.38268343] 79 | ]) 80 | 81 | def __init__(self): 82 | self._joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10) 83 | self._collision_boxes_pub = CollisionBoxesPublisher('franka_collision_boxes') 84 | 85 | self._collision_boxes_data = np.zeros((len(self.collision_box_shapes), 10)) 86 | self._collision_boxes_data[:, -3:] = self.collision_box_shapes 87 | 88 | # Precompute things and preallocate np memory for collision checking 89 | self._collision_box_poses = [] 90 | for pose in self._collision_box_poses_raw: 91 | T = np.eye(4) 92 | T[:3, 3] = pose[:3] 93 | T[:3, :3] = quaternion.as_rotation_matrix(quaternion.quaternion(*pose[3:])) 94 | self._collision_box_poses.append(T) 95 | 96 | self._collision_box_hdiags = [] 97 | self._collision_box_vertices_offset = [] 98 | self._vertex_offset_signs = np.array(list(product([1, -1],[1,-1], [1,-1]))) 99 | for sizes in self.collision_box_shapes: 100 | hsizes = sizes/2 101 | 102 | self._collision_box_vertices_offset.append(self._vertex_offset_signs * hsizes) 103 | self._collision_box_hdiags.append(np.linalg.norm(sizes/2)) 104 | self._collision_box_vertices_offset = np.array(self._collision_box_vertices_offset) 105 | self._collision_box_hdiags = np.array(self._collision_box_hdiags) 106 | 107 | self._collision_proj_axes = np.zeros((3, 15)) 108 | self._box_vertices_offset = np.ones([8, 3]) 109 | self._box_transform = np.eye(4) 110 | 111 | def forward_kinematics(self, joints): 112 | ''' 113 | Calculate the position of each joint using the dh_params 114 | Arguments: array of joint positions (rad) 115 | Returns: A numpy array that contains the 4x4 transformation matrices from the base to the position of each joint. 116 | ''' 117 | forward_kinematics = np.zeros((len(self.dh_params), 4, 4)) 118 | previous_transformation = np.eye(4) 119 | 120 | for i in range(len(self.dh_params)): 121 | a, d, alpha, theta = self.dh_params[i] 122 | 123 | if i < self.num_dof: 124 | theta = theta + joints[i] 125 | 126 | ca, sa = np.cos(alpha), np.sin(alpha) 127 | ct, st = np.cos(theta), np.sin(theta) 128 | self._dh_alpha_rot[1, 1] = ca 129 | self._dh_alpha_rot[1, 2] = -sa 130 | self._dh_alpha_rot[2, 1] = sa 131 | self._dh_alpha_rot[2, 2] = ca 132 | 133 | self._dh_a_trans[0, 3] = a 134 | self._dh_d_trans[2, 3] = d 135 | 136 | self._dh_theta_rot[0, 0] = ct 137 | self._dh_theta_rot[0, 1] = -st 138 | self._dh_theta_rot[1, 0] = st 139 | self._dh_theta_rot[1, 1] = ct 140 | 141 | transform = self._dh_alpha_rot.dot(self._dh_a_trans).dot(self._dh_d_trans).dot(self._dh_theta_rot) 142 | 143 | forward_kinematics[i] = previous_transformation.dot(transform) 144 | previous_transformation = forward_kinematics[i] 145 | 146 | return forward_kinematics 147 | 148 | def ee(self, joints): 149 | ''' 150 | Arguments: array of joint positions (rad) 151 | Returns: A numpy array that contains the [x, y, z, roll, pitch, yaw] location of the end-effector. 152 | ''' 153 | fk = self.forward_kinematics(joints) 154 | ee_frame = fk[-1,:,:] 155 | 156 | x, y, z = ee_frame[:-1,3] 157 | roll = np.arctan2(ee_frame[2,1], ee_frame[2,2]) 158 | pitch = np.arcsin(-ee_frame[2,0]) 159 | yaw = np.arctan2(ee_frame[1,0], ee_frame[0,0]) 160 | 161 | return np.array([x, y, z, roll, pitch, yaw]) 162 | 163 | def jacobian(self, joints): 164 | ''' 165 | Calculate the jacobians analytically using your forward kinematics 166 | Arguments: array of joint positions (rad) 167 | Returns: A numpy array that contains the 6 x num_dof end-effector jacobian. 168 | ''' 169 | jacobian = np.zeros((6,self.num_dof)) 170 | fk = self.forward_kinematics(joints) 171 | ee_pos = fk[-1,:3,3] 172 | 173 | for i in range(self.num_dof): 174 | joint_pos = fk[i,:3,3] 175 | joint_axis = fk[i,:3,2] 176 | jacobian[:3,i] = np.cross(joint_axis, (ee_pos - joint_pos)).T 177 | jacobian[3:,i] = joint_axis.T 178 | 179 | return jacobian 180 | 181 | def inverse_kinematics(self, desired_ee_pos, current_joints): 182 | ''' 183 | Arguments: desired_ee_pos which is a np array of [x, y, z, r, p, y] which represent the desired end-effector position of the robot 184 | current_joints which represents the current location of the robot 185 | Returns: A numpy array that contains the joints required in order to achieve the desired end-effector position. 186 | ''' 187 | joints = current_joints.copy() 188 | current_ee_pos = self.ee(joints) 189 | ee_error = desired_ee_pos - current_ee_pos 190 | alpha = 0.1 191 | 192 | while np.linalg.norm(ee_error) > 1e-3: 193 | jacob = self.jacobian(joints) 194 | joints += alpha * jacob.T.dot(ee_error.T) 195 | 196 | current_ee_pos = self.ee(joints) 197 | ee_error = desired_ee_pos - current_ee_pos 198 | 199 | return joints 200 | 201 | def check_box_collision(self, joints, box): 202 | ''' 203 | Arguments: joints represents the current location of the robot 204 | box contains the position of the center of the box [x, y, z, r, p, y] and the length, width, and height [l, w, h] 205 | Returns: A boolean where True means the box is in collision with the arm and false means that there are no collisions. 206 | ''' 207 | box_pos, box_rpy, box_hsizes = box[:3], box[3:6], box[6:]/2 208 | box_q = quaternion.from_euler_angles(box_rpy) 209 | box_axes = quaternion.as_rotation_matrix(box_q) 210 | 211 | self._box_vertices_offset[:,:] = self._vertex_offset_signs * box_hsizes 212 | box_vertices = (box_axes.dot(self._box_vertices_offset.T) + np.expand_dims(box_pos, 1)).T 213 | 214 | box_hdiag = np.linalg.norm(box_hsizes) 215 | min_col_dists = box_hdiag + self._collision_box_hdiags 216 | 217 | franka_box_poses = self.get_collision_boxes_poses(joints) 218 | for i, franka_box_pose in enumerate(franka_box_poses): 219 | fbox_pos = franka_box_pose[:3, 3] 220 | fbox_axes = franka_box_pose[:3, :3] 221 | 222 | # coarse collision check 223 | if np.linalg.norm(fbox_pos - box_pos) > min_col_dists[i]: 224 | continue 225 | 226 | fbox_vertex_offsets = self._collision_box_vertices_offset[i] 227 | fbox_vertices = fbox_vertex_offsets.dot(fbox_axes.T) + fbox_pos 228 | 229 | # construct axes 230 | cross_product_pairs = np.array(list(product(box_axes.T, fbox_axes.T))) 231 | cross_axes = np.cross(cross_product_pairs[:,0], cross_product_pairs[:,1]).T 232 | self._collision_proj_axes[:, :3] = box_axes 233 | self._collision_proj_axes[:, 3:6] = fbox_axes 234 | self._collision_proj_axes[:, 6:] = cross_axes 235 | 236 | # projection 237 | box_projs = box_vertices.dot(self._collision_proj_axes) 238 | fbox_projs = fbox_vertices.dot(self._collision_proj_axes) 239 | min_box_projs, max_box_projs = box_projs.min(axis=0), box_projs.max(axis=0) 240 | min_fbox_projs, max_fbox_projs = fbox_projs.min(axis=0), fbox_projs.max(axis=0) 241 | 242 | # check if no separating planes exist 243 | if np.all([min_box_projs <= max_fbox_projs, max_box_projs >= min_fbox_projs]): 244 | return True 245 | 246 | return False 247 | 248 | def publish_joints(self, joints): 249 | joint_state = JointState() 250 | joint_state.name = ['panda_joint1', 'panda_joint2', 'panda_joint3', 'panda_joint4', 'panda_joint5', 'panda_joint6', 'panda_joint7', 'panda_finger_joint1', 'panda_finger_joint2'] 251 | joint_state.header.stamp = rospy.Time.now() 252 | 253 | if len(joints) == 7: 254 | joints = np.concatenate([joints, [0, 0]]) 255 | joint_state.position = joints 256 | 257 | self._joint_state_pub.publish(joint_state) 258 | 259 | def get_collision_boxes_poses(self, joints): 260 | fk = self.forward_kinematics(joints) 261 | 262 | box_poses_world = [] 263 | for i, link in enumerate(self._collision_box_links): 264 | link_transform = fk[link - 1] 265 | box_pose_world = link_transform.dot(self._collision_box_poses[i]) 266 | box_poses_world.append(box_pose_world) 267 | 268 | return box_poses_world 269 | 270 | def publish_collision_boxes(self, joints): 271 | box_poses_world = self.get_collision_boxes_poses(joints) 272 | 273 | for i, pose in enumerate(box_poses_world): 274 | self._collision_boxes_data[i, :3] = pose[:3, 3] 275 | q = quaternion.from_rotation_matrix(pose[:3, :3]) 276 | 277 | for j, k in enumerate('wxyz'): 278 | self._collision_boxes_data[i, 3 + j] = getattr(q, k) 279 | 280 | self._collision_boxes_pub.publish_boxes(self._collision_boxes_data) 281 | -------------------------------------------------------------------------------- /Path Planning/franka_visualization.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 89 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /RobotModel1 8 | - /Axes1 9 | - /MarkerArray1 10 | - /MarkerArray2 11 | Splitter Ratio: 0.5 12 | Tree Height: 764 13 | - Class: rviz/Selection 14 | Name: Selection 15 | - Class: rviz/Tool Properties 16 | Expanded: 17 | - /2D Pose Estimate1 18 | - /2D Nav Goal1 19 | - /Publish Point1 20 | Name: Tool Properties 21 | Splitter Ratio: 0.588679016 22 | - Class: rviz/Views 23 | Expanded: 24 | - /Current View1 25 | Name: Views 26 | Splitter Ratio: 0.5 27 | - Class: rviz/Time 28 | Experimental: false 29 | Name: Time 30 | SyncMode: 0 31 | SyncSource: "" 32 | Toolbars: 33 | toolButtonStyle: 2 34 | Visualization Manager: 35 | Class: "" 36 | Displays: 37 | - Alpha: 0.5 38 | Cell Size: 1 39 | Class: rviz/Grid 40 | Color: 160; 160; 164 41 | Enabled: true 42 | Line Style: 43 | Line Width: 0.0299999993 44 | Value: Lines 45 | Name: Grid 46 | Normal Cell Count: 0 47 | Offset: 48 | X: 0 49 | Y: 0 50 | Z: 0 51 | Plane: XY 52 | Plane Cell Count: 10 53 | Reference Frame: 54 | Value: true 55 | - Alpha: 1 56 | Class: rviz/RobotModel 57 | Collision Enabled: false 58 | Enabled: true 59 | Links: 60 | All Links Enabled: true 61 | Expand Joint Details: false 62 | Expand Link Details: false 63 | Expand Tree: false 64 | Link Tree Style: Links in Alphabetic Order 65 | panda_hand: 66 | Alpha: 1 67 | Show Axes: false 68 | Show Trail: false 69 | Value: true 70 | panda_leftfinger: 71 | Alpha: 1 72 | Show Axes: false 73 | Show Trail: false 74 | Value: true 75 | panda_link0: 76 | Alpha: 1 77 | Show Axes: false 78 | Show Trail: false 79 | Value: true 80 | panda_link1: 81 | Alpha: 1 82 | Show Axes: false 83 | Show Trail: false 84 | Value: true 85 | panda_link2: 86 | Alpha: 1 87 | Show Axes: false 88 | Show Trail: false 89 | Value: true 90 | panda_link3: 91 | Alpha: 1 92 | Show Axes: false 93 | Show Trail: false 94 | Value: true 95 | panda_link4: 96 | Alpha: 1 97 | Show Axes: false 98 | Show Trail: false 99 | Value: true 100 | panda_link5: 101 | Alpha: 1 102 | Show Axes: false 103 | Show Trail: false 104 | Value: true 105 | panda_link6: 106 | Alpha: 1 107 | Show Axes: false 108 | Show Trail: false 109 | Value: true 110 | panda_link7: 111 | Alpha: 1 112 | Show Axes: false 113 | Show Trail: false 114 | Value: true 115 | panda_link8: 116 | Alpha: 1 117 | Show Axes: false 118 | Show Trail: false 119 | panda_rightfinger: 120 | Alpha: 1 121 | Show Axes: false 122 | Show Trail: false 123 | Value: true 124 | Name: RobotModel 125 | Robot Description: robot_description 126 | TF Prefix: "" 127 | Update Interval: 0 128 | Value: true 129 | Visual Enabled: true 130 | - Class: rviz/Axes 131 | Enabled: true 132 | Length: 0.200000003 133 | Name: Axes 134 | Radius: 0.0500000007 135 | Reference Frame: 136 | Value: true 137 | - Class: rviz/MarkerArray 138 | Enabled: true 139 | Marker Topic: /franka_collision_boxes 140 | Name: MarkerArray 141 | Namespaces: 142 | "": true 143 | Queue Size: 100 144 | Value: true 145 | - Class: rviz/MarkerArray 146 | Enabled: true 147 | Marker Topic: /collision_boxes 148 | Name: MarkerArray 149 | Namespaces: 150 | "": true 151 | Queue Size: 100 152 | Value: true 153 | Enabled: true 154 | Global Options: 155 | Background Color: 48; 48; 48 156 | Default Light: true 157 | Fixed Frame: panda_link0 158 | Frame Rate: 30 159 | Name: root 160 | Tools: 161 | - Class: rviz/Interact 162 | Hide Inactive Objects: true 163 | - Class: rviz/MoveCamera 164 | - Class: rviz/Select 165 | - Class: rviz/FocusCamera 166 | - Class: rviz/Measure 167 | - Class: rviz/SetInitialPose 168 | Topic: /initialpose 169 | - Class: rviz/SetGoal 170 | Topic: /move_base_simple/goal 171 | - Class: rviz/PublishPoint 172 | Single click: true 173 | Topic: /clicked_point 174 | Value: true 175 | Views: 176 | Current: 177 | Class: rviz/Orbit 178 | Distance: 2.64484072 179 | Enable Stereo Rendering: 180 | Stereo Eye Separation: 0.0599999987 181 | Stereo Focal Distance: 1 182 | Swap Stereo Eyes: false 183 | Value: false 184 | Focal Point: 185 | X: 0.121937089 186 | Y: 0.0486892052 187 | Z: 0.405003011 188 | Focal Shape Fixed Size: false 189 | Focal Shape Size: 0.0500000007 190 | Invert Z Axis: false 191 | Name: Current View 192 | Near Clip Distance: 0.00999999978 193 | Pitch: 0.324795991 194 | Target Frame: 195 | Value: Orbit (rviz) 196 | Yaw: 0.308233857 197 | Saved: ~ 198 | Window Geometry: 199 | Displays: 200 | collapsed: false 201 | Height: 1056 202 | Hide Left Dock: false 203 | Hide Right Dock: false 204 | QMainWindow State: 000000ff00000000fd00000004000000000000016a00000396fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006100fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000002800000396000000d700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000396fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000002800000396000000ad00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb0000039600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 205 | Selection: 206 | collapsed: false 207 | Time: 208 | collapsed: false 209 | Tool Properties: 210 | collapsed: false 211 | Views: 212 | collapsed: false 213 | Width: 1920 214 | X: 0 215 | Y: 24 216 | -------------------------------------------------------------------------------- /Path Planning/kdtree.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | 3 | ''' From https://pypi.org/project/KdQuery/ 4 | ''' 5 | 6 | """Kd-tree implementation. 7 | 8 | This module defines one possible kd-tree structure implementation and a 9 | general method to find the nearest node for any kd-tree implementation. 10 | 11 | """ 12 | import math 13 | from collections import deque, namedtuple 14 | 15 | 16 | def interval_condition(value, inf, sup, dist): 17 | """Checks if value belongs to the interval [inf - dist, sup + dist]. 18 | """ 19 | return inf - dist < value < sup + dist 20 | 21 | 22 | def euclidean_dist(point1, point2): 23 | return math.sqrt(sum([math.pow(point1[i] - point2[i], 2) 24 | for i in range(len(point1))])) 25 | 26 | 27 | Node = namedtuple('Node', 'point region axis active left right data') 28 | """Internal representation of a node. 29 | 30 | The tree is represented by a list of node. Each node is associated to a 31 | point in the k-dimensional space, is inside a region, devises this region 32 | in two parts according to an axis, has two child nodes and stores some 33 | data. 34 | 35 | """ 36 | 37 | 38 | class KDTree: 39 | """Kd-tree implementation. 40 | 41 | This class defines one implementation of a kd-tree using a python list to 42 | save the methods from recursion. 43 | 44 | Args: 45 | k (int, optional): The number of dimensions of the space. 46 | capacity (int, optional): The maximum number of nodes in the tree. 47 | limits (:obj:`list` of :obj:`list` of float or int, optional): A list 48 | of size k, where each list contains two numbers defining the limits 49 | of the region which all the nodes will be. If none is passed as 50 | argument, the region will be all the space, that is, ]-inf, inf[ 51 | for each one of the k axis. 52 | 53 | Attributes: 54 | node_list (:obj:`list` of :obj:Node): The list of nodes. 55 | size (int): The number of active nodes in the list. 56 | next_identifier (int): The identifier of the next node to be inserted 57 | in the list. 58 | k (int): The number of dimensions of the space. 59 | region (:obj:`list` of :obj:`list` of float or int, optional): A list 60 | of size k, where each list contains two numbers defining the limits 61 | of the region which all the nodes belong to. If none is passed as 62 | argument, the region will be all the space, that is, ]-inf, inf[ 63 | for each one of the k axis. 64 | 65 | """ 66 | 67 | def __init__(self, k=2, capacity=10000, limits=None): 68 | self.node_list = [None] * capacity 69 | self.size = 0 70 | self.next_identifier = 0 71 | self.k = k 72 | 73 | # The region of the space where all the points are 74 | self.region = limits if limits is not None \ 75 | else [[-math.inf, math.inf]] * k 76 | 77 | def __len__(self): 78 | return self.size 79 | 80 | def __iter__(self): 81 | return (node for node in self.node_list 82 | if node is not None and node.active) 83 | 84 | def get_node(self, node_id): 85 | return self.node_list[node_id] 86 | 87 | def deactivate(self, node_id): 88 | """Deactivate the node identified by node_id. 89 | 90 | Deactivates the node corresponding to node_id, which means that 91 | it can never be the output of a nearest_point query. 92 | 93 | Note: 94 | The node is not removed from the tree, its data is steel available. 95 | 96 | Args: 97 | node_id (int): The node identifier (given to the user after 98 | its insertion). 99 | 100 | """ 101 | node = self.node_list[node_id] 102 | self.node_list[node_id] = node._replace(active=False) 103 | 104 | def insert(self, point, data=None): 105 | """Insert a new node in the tree. 106 | 107 | Args: 108 | point (:obj:`tuple` of float or int): Stores the position of the 109 | node. 110 | data (:obj, optional): The information stored by the node. 111 | 112 | Returns: 113 | int: The identifier of the new node. 114 | 115 | Example: 116 | >>> tree = Tree(4, 800) 117 | >>> point = (3, 7) 118 | >>> data = {'name': Fresnel, 'label': blue, 'speed': 98.2} 119 | >>> node_id = tree.insert(point, data) 120 | 121 | """ 122 | assert len(point) == self.k 123 | 124 | if self.size == 0: 125 | if self.region is None: 126 | self.region = [[-math.inf, math.inf]] * self.k 127 | axis = 0 128 | return self.new_node(point, self.region, axis, data) 129 | 130 | # Iteratively descends to one leaf 131 | current_id = 0 132 | while True: 133 | parent_node = self.node_list[current_id] 134 | axis = parent_node.axis 135 | if point[axis] < parent_node.point[axis]: 136 | next_id, left = parent_node.left, True 137 | else: 138 | next_id, left = parent_node.right, False 139 | 140 | if next_id is None: 141 | break 142 | 143 | current_id = next_id 144 | 145 | # Get the region delimited by the parent node 146 | region = parent_node.region[:] 147 | region[axis] = parent_node.region[axis][:] 148 | 149 | # Limit to the child's region 150 | limit = parent_node.point[axis] 151 | 152 | # Update reference to the new node 153 | if left: 154 | self.node_list[current_id] = parent_node._replace(left=self.size) 155 | region[axis][1] = limit 156 | else: 157 | self.node_list[current_id] = parent_node._replace(right=self.size) 158 | region[axis][0] = limit 159 | 160 | return self.new_node(point, region, (axis + 1) % self.k, data) 161 | 162 | def new_node(self, point, region, axis, data): 163 | node = Node(point, region, axis, True, None, None, data) 164 | 165 | # Identifier to new node 166 | node_id = self.next_identifier 167 | self.node_list[node_id] = node 168 | 169 | self.size += 1 170 | self.next_identifier += 1 171 | 172 | return node_id 173 | 174 | def find_nearest_point(self, query, dist_fun=euclidean_dist): 175 | """Find the point in the tree that minimizes the distance to the query. 176 | 177 | Args: 178 | query (:obj:`tuple` of float or int): Stores the position of the 179 | node. 180 | dist_fun (:obj:`function`, optional): The distance function, 181 | euclidean distance by default. 182 | 183 | Returns: 184 | :obj:`tuple`: Tuple of length 2, where the first element is the 185 | identifier of the nearest node, the second is the distance 186 | to the query. 187 | 188 | Example: 189 | >>> tree = Tree(2, 3) 190 | >>> tree.insert((0, 0)) 191 | >>> tree.insert((3, 5)) 192 | >>> tree.insert((-1, 7)) 193 | >>> query = (-1, 8) 194 | >>> nearest_node_id, dist = tree.find_nearest_point(query) 195 | >>> dist 196 | 1 197 | 198 | """ 199 | def get_properties(node_id): 200 | return self.node_list[node_id][:6] 201 | 202 | return nearest_point(query, 0, get_properties, dist_fun) 203 | 204 | 205 | def nearest_point(query, root_id, get_properties, dist_fun=euclidean_dist): 206 | """Find the point in the tree that minimizes the distance to the query. 207 | 208 | This method implements the nearest_point query for any structure 209 | implementing a kd-tree. The only requirement is a function capable to 210 | extract the relevant properties from a node representation of the 211 | particular implementation. 212 | 213 | Args: 214 | query (:obj:`tuple` of float or int): Stores the position of the 215 | node. 216 | root_id (:obj): The identifier of the root in the kd-tree 217 | implementation. 218 | get_properties (:obj:`function`): The function to extract the 219 | relevant properties from a node, namely its point, region, 220 | axis, left child identifier, right child identifier and 221 | if it is active. If the implementation does not uses 222 | the active attribute the function should return always True. 223 | dist_fun (:obj:`function`, optional): The distance function, 224 | euclidean distance by default. 225 | 226 | Returns: 227 | :obj:`tuple`: Tuple of length 2, where the first element is the 228 | identifier of the nearest node, the second is the distance 229 | to the query. 230 | 231 | """ 232 | 233 | k = len(query) 234 | dist = math.inf 235 | 236 | nearest_node_id = None 237 | 238 | # stack_node: stack of identifiers to nodes within a region that 239 | # contains the query. 240 | # stack_look: stack of identifiers to nodes within a region that 241 | # does not contains the query. 242 | stack_node = deque([root_id]) 243 | stack_look = deque() 244 | 245 | while stack_node or stack_look: 246 | 247 | if stack_node: 248 | node_id = stack_node.pop() 249 | look_node = False 250 | else: 251 | node_id = stack_look.pop() 252 | look_node = True 253 | 254 | point, region, axis, active, left, right = get_properties(node_id) 255 | 256 | # Should consider this node? 257 | # As it is within a region that does not contains the query, maybe 258 | # there is no chance to find a closer node in this region 259 | if look_node: 260 | inside_region = True 261 | for i in range(k): 262 | inside_region &= interval_condition(query[i], region[i][0], 263 | region[i][1], dist) 264 | if not inside_region: 265 | continue 266 | 267 | # Update the distance only if the node is active. 268 | if active: 269 | node_distance = dist_fun(query, point) 270 | if nearest_node_id is None or dist > node_distance: 271 | nearest_node_id = node_id 272 | dist = node_distance 273 | 274 | if query[axis] < point[axis]: 275 | side_node = left 276 | side_look = right 277 | else: 278 | side_node = right 279 | side_look = left 280 | 281 | if side_node is not None: 282 | stack_node.append(side_node) 283 | 284 | if side_look is not None: 285 | stack_look.append(side_look) 286 | 287 | return nearest_node_id, dist 288 | -------------------------------------------------------------------------------- /Path Planning/requirements.txt: -------------------------------------------------------------------------------- 1 | numpy 2 | rospkg 3 | numpy-quaternion -------------------------------------------------------------------------------- /Path Planning/rrt.py: -------------------------------------------------------------------------------- 1 | from time import time 2 | import numpy as np 3 | 4 | from kdtree import KDTree 5 | from franka_robot import FrankaRobot 6 | 7 | 8 | class SimpleTree: 9 | 10 | def __init__(self, dim): 11 | self._parents_map = {} 12 | self._kd = KDTree(dim) 13 | 14 | def insert_new_node(self, point, parent=None): 15 | node_id = self._kd.insert(point) 16 | self._parents_map[node_id] = parent 17 | 18 | return node_id 19 | 20 | def get_parent(self, child_id): 21 | return self._parents_map[child_id] 22 | 23 | def get_point(self, node_id): 24 | return self._kd.get_node(node_id).point 25 | 26 | def get_nearest_node(self, point): 27 | return self._kd.find_nearest_point(point) 28 | 29 | 30 | class RRT: 31 | 32 | def __init__(self, fr, is_in_collision): 33 | self._fr = fr 34 | self._is_in_collision = is_in_collision 35 | 36 | ''' 37 | TODO: You can tune these parameters to improve RRT performance. 38 | 39 | However, make sure the values satisfy the following conditions: 40 | self._constraint_th < 2e-3 41 | self._q_step_size < 0.1 42 | ''' 43 | self._project_step_size = 1e-1 # Default:1e-1 44 | self._constraint_th = 1e-3 # Default: 1e-3 45 | 46 | self._q_step_size = 0.02 # Default: 0.01 47 | self._target_p = 0.2 # Default: 0.3 48 | self._max_n_nodes = int(1e5) 49 | 50 | def sample_valid_joints(self): 51 | ''' 52 | TODO: Implement sampling a random valid configuration. 53 | 54 | The sampled configuration must be within the joint limits, but it does not check for collisions. 55 | 56 | Please use the following in your code: 57 | self._fr.joint_limis_low - lower joint limits 58 | self._fr.joint_limis_high - higher joint limits 59 | self._fr.num_dof - the degree of freedom of franka 60 | ''' 61 | # Random Sample [1, num_dof] in the configuration space between lower joint limits and higher joint limits 62 | q = (self._fr.joint_limits_high - self._fr.joint_limits_low) * np.random.random(self._fr.num_dof) + self._fr.joint_limits_low 63 | return q 64 | 65 | def project_to_constraint(self, q, constraint): 66 | ''' 67 | TODO: Implement projecting a configuration to satisfy a constraint function using gradient descent. 68 | 69 | Please use the following parameters in your code: 70 | self._project_step_size - learning rate for gradient descent 71 | self._constraint_th - a threshold lower than which the constraint is considered to be satisfied 72 | 73 | Input: 74 | q - the point to be projected 75 | constraint - a function of q that returns (constraint_value, constraint_gradient) 76 | constraint_value is a scalar - it is 0 when the constraint is satisfied 77 | constraint_gradient is a vector of length 6 - it is the gradient of the 78 | constraint value w.r.t. the end-effector pose (x, y, z, r, p, y) 79 | 80 | Output: 81 | q_proj - the projected point 82 | 83 | You can obtain the Jacobian by calling self._fr.jacobian(q) 84 | ''' 85 | q_proj = q.copy() 86 | err, grad = constraint(q) 87 | while err > self._constraint_th: 88 | # print('The error is: ', err) 89 | J = self._fr.jacobian(q_proj) 90 | q_proj -= self._project_step_size * J.T.dot(grad) 91 | # q_proj -= self._project_step_size * J.T.dot(np.linalg.inv(J.dot(J.T))).dot(grad) 92 | err, grad = constraint(q_proj) 93 | return q_proj 94 | 95 | def extend(self, tree, q_target, constraint=None): 96 | ''' 97 | TODO: Implement the constraint extend function. 98 | 99 | Input: 100 | tree - a SimpleTree object containing existing nodes 101 | q_target - configuration of the target state, in shape of [1, num_dof] 102 | constraint - a constraint function used by project_to_constraint 103 | do not perform projection if constraint is None 104 | 105 | Output: 106 | target_reached - bool, whether or not the target has been reached 107 | new_node_id - node_id of the new node inserted into the tree by this extend operation 108 | Note: tree.insert_new_node returns a node_id 109 | ''' 110 | target_reached = False 111 | new_node_id = None 112 | is_collision = True 113 | 114 | while is_collision: 115 | if np.random.random(1) < self._target_p: 116 | # Make sure it will approach to the target 117 | q_sample = q_target 118 | else: 119 | q_sample = self.sample_valid_joints() 120 | 121 | # Find the nearest node (q_near) of the sampling point in current nodes tree 122 | # Make a step from the nearest node (q_near) to become a new node (q_new) and expand the nodes tree 123 | nearest_node_id = tree.get_nearest_node(q_sample)[0] 124 | q_near = tree.get_point(nearest_node_id) 125 | q_new = q_near + min(self._q_step_size, np.linalg.norm(q_sample - q_near)) * (q_sample - q_near) / np.linalg.norm(q_sample - q_near) 126 | 127 | # Check if the new node has collision with the constraint 128 | q_new = self.project_to_constraint(q_new, constraint) 129 | 130 | if self._is_in_collision(q_new): 131 | is_collision = True 132 | continue 133 | else: 134 | is_collision = False 135 | 136 | # Add the q_new as vertex, and the edge between q_new and q_near as edge to the tree 137 | new_node_id = tree.insert_new_node(q_new, nearest_node_id) 138 | 139 | # if the new state (q_new) is close to the target state, then we reached the target state 140 | if np.linalg.norm(q_new - q_target) < self._q_step_size: 141 | target_reached = True 142 | 143 | return target_reached, new_node_id 144 | 145 | def plan(self, q_start, q_target, constraint=None): 146 | tree = SimpleTree(len(q_start)) 147 | tree.insert_new_node(q_start) 148 | 149 | s = time() 150 | for n_nodes_sampled in range(self._max_n_nodes): 151 | if n_nodes_sampled > 0 and n_nodes_sampled % 100 == 0: 152 | print('RRT: Sampled {} nodes'.format(n_nodes_sampled)) 153 | 154 | reached_target, node_id_new = self.extend(tree, q_target, constraint) 155 | 156 | if reached_target: 157 | break 158 | 159 | print('RRT: Sampled {} nodes in {:.2f}s'.format(n_nodes_sampled, time() - s)) 160 | 161 | path = [] 162 | if reached_target: 163 | backward_path = [q_target] 164 | node_id = node_id_new 165 | while node_id is not None: 166 | backward_path.append(tree.get_point(node_id)) 167 | node_id = tree.get_parent(node_id) 168 | path = backward_path[::-1] 169 | 170 | print('RRT: Found a path! Path length is {}.'.format(len(path))) 171 | else: 172 | print('RRT: Was not able to find a path!') 173 | 174 | return path 175 | -------------------------------------------------------------------------------- /Path Planning/rrt_connect.py: -------------------------------------------------------------------------------- 1 | from time import time 2 | import numpy as np 3 | from scipy.optimize import minimize, LinearConstraint, NonlinearConstraint 4 | 5 | from kdtree import KDTree 6 | from franka_robot import FrankaRobot 7 | 8 | 9 | class SimpleTree: 10 | 11 | def __init__(self, dim): 12 | self._parents_map = {} 13 | self._kd = KDTree(dim) 14 | 15 | def insert_new_node(self, point, parent=None): 16 | node_id = self._kd.insert(point) 17 | self._parents_map[node_id] = parent 18 | 19 | return node_id 20 | 21 | def get_parent(self, child_id): 22 | return self._parents_map[child_id] 23 | 24 | def get_point(self, node_id): 25 | return self._kd.get_node(node_id).point 26 | 27 | def get_nearest_node(self, point): 28 | return self._kd.find_nearest_point(point) 29 | 30 | def construct_path_to_root(self, leaf_node_id): 31 | path = [] 32 | node_id = leaf_node_id 33 | while node_id is not None: 34 | path.append(self.get_point(node_id)) 35 | node_id = self.get_parent(node_id) 36 | 37 | return path 38 | 39 | 40 | class RRTConnect: 41 | 42 | def __init__(self, fr, is_in_collision): 43 | self._fr = fr 44 | self._is_in_collision = is_in_collision 45 | 46 | self._q_step_size = 0.02 47 | self._connect_dist = 0.8 48 | self._max_n_nodes = int(1e5) 49 | 50 | def sample_valid_joints(self): 51 | q = np.random.random(self._fr.num_dof) * (self._fr.joint_limits_high - self._fr.joint_limits_low) + self._fr.joint_limits_low 52 | return q 53 | 54 | def project_to_constraint(self, q0, constraint): 55 | def f(q): 56 | return constraint(q)[0] 57 | 58 | def df(q): 59 | c_grad = constraint(q)[1] 60 | q_grad = self._fr.jacobian(q).T @ c_grad 61 | return q_grad 62 | 63 | def c_f(q): 64 | diff_q = q - q0 65 | return diff_q @ diff_q 66 | 67 | def c_df(q): 68 | diff_q = q - q0 69 | return 0.5 * diff_q 70 | 71 | c_joint_limits = LinearConstraint(np.eye(len(q0)), self._fr.joint_limits_low, self._fr.joint_limits_high) 72 | c_close_to_q0 = NonlinearConstraint(c_f, 0, self._q_step_size ** 2, jac=c_df) 73 | 74 | res = minimize(f, q0, jac=df, method='SLSQP', tol=0.1, 75 | constraints=(c_joint_limits, c_close_to_q0)) 76 | 77 | return res.x 78 | 79 | def _is_seg_valid(self, q0, q1): 80 | qs = np.linspace(q0, q1, int(np.linalg.norm(q1 - q0) / self._q_step_size)) 81 | for q in qs: 82 | if self._is_in_collision(q): 83 | return False 84 | return True 85 | 86 | def extend(self, tree_0, tree_1, constraint=None): 87 | ''' 88 | TODO: Implement extend for RRT Connect 89 | 90 | - Only perform self.project_to_constraint if constraint is not None 91 | - Use self._is_seg_valid, self._q_step_size, self._connect_dist 92 | ''' 93 | target_reached = False 94 | node_id_new = None 95 | is_collision = True 96 | 97 | while is_collision: 98 | q_sample = self.sample_valid_joints() 99 | 100 | node_id_near = tree_0.get_nearest_node(q_sample)[0] 101 | q_near = tree_0.get_point(node_id_near) 102 | q_new = q_near + min(self._q_step_size, np.linalg.norm(q_sample - q_near)) * (q_sample - q_near) / np.linalg.norm(q_sample - q_near) 103 | 104 | q_new = self.project_to_constraint(q_new, constraint) 105 | 106 | if self._is_in_collision(q_new): 107 | is_collision = True 108 | continue 109 | else: 110 | is_collision = False 111 | 112 | # Add the q_new as vertex, and the edge between q_new and q_near as edge to the tree 113 | node_id_new = tree_0.insert_new_node(q_new, node_id_near) 114 | node_id_1 = tree_1.get_nearest_node(q_new)[0] 115 | q_1 = tree_1.get_point(node_id_1) 116 | # if the new state is close to the target state, then we reached the target state 117 | if np.linalg.norm(q_new - q_1) < self._connect_dist and self._is_seg_valid(q_new, q_1): 118 | target_reached = True 119 | 120 | return target_reached, node_id_new, node_id_1 121 | 122 | def plan(self, q_start, q_target, constraint=None): 123 | tree_0 = SimpleTree(len(q_start)) 124 | tree_0.insert_new_node(q_start) 125 | 126 | tree_1 = SimpleTree(len(q_target)) 127 | tree_1.insert_new_node(q_target) 128 | 129 | q_start_is_tree_0 = True 130 | 131 | s = time() 132 | for n_nodes_sampled in range(self._max_n_nodes): 133 | if n_nodes_sampled > 0 and n_nodes_sampled % 100 == 0: 134 | print('RRT: Sampled {} nodes'.format(n_nodes_sampled)) 135 | 136 | reached_target, node_id_new, node_id_1 = self.extend(tree_0, tree_1, constraint) 137 | 138 | if reached_target: 139 | break 140 | 141 | q_start_is_tree_0 = not q_start_is_tree_0 142 | tree_0, tree_1 = tree_1, tree_0 143 | 144 | print('RRT: Sampled {} nodes in {:.2f}s'.format(n_nodes_sampled, time() - s)) 145 | 146 | if not q_start_is_tree_0: 147 | tree_0, tree_1 = tree_1, tree_0 148 | 149 | if reached_target: 150 | tree_0_backward_path = tree_0.construct_path_to_root(node_id_new) 151 | tree_1_forward_path = tree_1.construct_path_to_root(node_id_1) 152 | 153 | q0 = tree_0_backward_path[0] 154 | q1 = tree_1_forward_path[0] 155 | tree_01_connect_path = np.linspace(q0, q1, int(np.linalg.norm(q1 - q0) / self._q_step_size))[1:].tolist() 156 | 157 | path = tree_0_backward_path[::-1] + tree_01_connect_path + tree_1_forward_path 158 | print('RRT: Found a path! Path length is {}.'.format(len(path))) 159 | else: 160 | path = [] 161 | print('RRT: Was not able to find a path!') 162 | 163 | return np.array(path).tolist() 164 | -------------------------------------------------------------------------------- /Path Planning/test_rrt.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import rospy 3 | 4 | from franka_robot import FrankaRobot 5 | from collision_boxes_publisher import CollisionBoxesPublisher 6 | from rrt import RRT 7 | 8 | 9 | if __name__ == '__main__': 10 | np.random.seed(0) 11 | rospy.init_node('rrt') 12 | fr = FrankaRobot() 13 | 14 | boxes = np.array([ 15 | # obstacle 16 | [0.4, 0, 0.25, 0, 0, 0, 0.3, 0.05, 0.5], 17 | # sides 18 | [0.15, 0.46, 0.5, 0, 0, 0, 1.2, 0.01, 1.1], 19 | [0.15, -0.46, 0.5, 0, 0, 0, 1.2, 0.01, 1.1], 20 | # back 21 | [-0.41, 0, 0.5, 0, 0, 0, 0.01, 1, 1.1], 22 | # front 23 | [0.75, 0, 0.5, 0, 0, 0, 0.01, 1, 1.1], 24 | # top 25 | [0.2, 0, 1, 0, 0, 0, 1.2, 1, 0.01], 26 | # bottom 27 | [0.2, 0, -0.05, 0, 0, 0, 1.2, 1, 0.01] 28 | ]) 29 | def is_in_collision(joints): 30 | for box in boxes: 31 | if fr.check_box_collision(joints, box): 32 | return True 33 | return False 34 | 35 | desired_ee_rp = fr.ee(fr.home_joints)[3:5] 36 | def ee_upright_constraint(q): 37 | ''' 38 | TODO: Implement constraint function and its gradient. 39 | 40 | This constraint should enforce the end-effector stays upright. 41 | Hint: Use the roll and pitch angle in desired_ee_rp. The end-effector is upright in its home state. 42 | 43 | Input: 44 | q - a joint configuration 45 | 46 | Output: 47 | err - a non-negative scalar that is 0 when the constraint is satisfied 48 | grad - a vector of length 6, where the ith element is the derivative of err w.r.t. the ith element of ee 49 | ''' 50 | ee = fr.ee(q) 51 | err = np.sum((np.asarray(desired_ee_rp) - np.asarray(ee[3:5]))**2) 52 | grad = np.asarray([0, 0, 0, 2*(ee[3]-desired_ee_rp[0]), 2*(ee[4]-desired_ee_rp[1]), 0]) 53 | return err, grad 54 | 55 | print('Desired ee roll and pitch:', desired_ee_rp) 56 | ''' 57 | TODO: Fill in start and target joint positions 58 | ''' 59 | joints_start = fr.home_joints.copy() 60 | joints_start[0] = -np.deg2rad(45) 61 | joints_target = joints_start.copy() 62 | joints_target[0] = np.deg2rad(45) 63 | 64 | rrt = RRT(fr, is_in_collision) 65 | constraint = ee_upright_constraint 66 | plan = rrt.plan(joints_start, joints_target, constraint) 67 | 68 | i = 0 69 | collision_boxes_publisher = CollisionBoxesPublisher('collision_boxes') 70 | rate = rospy.Rate(10) 71 | while not rospy.is_shutdown(): 72 | joints = plan[i % len(plan)] 73 | fr.publish_joints(joints) 74 | fr.publish_collision_boxes(joints) 75 | collision_boxes_publisher.publish_boxes(boxes) 76 | 77 | i += 1 78 | rate.sleep() 79 | -------------------------------------------------------------------------------- /Path Planning/test_rrt_connect.py: -------------------------------------------------------------------------------- 1 | import argparse 2 | import numpy as np 3 | import rospy 4 | 5 | from franka_robot import FrankaRobot 6 | from collision_boxes_publisher import CollisionBoxesPublisher 7 | from rrt_connect import RRTConnect 8 | 9 | 10 | if __name__ == '__main__': 11 | parser = argparse.ArgumentParser() 12 | parser.add_argument('--seed', '-s', type=int, default=0) 13 | args = parser.parse_args() 14 | 15 | np.random.seed(args.seed) 16 | fr = FrankaRobot() 17 | 18 | rospy.init_node('rrt') 19 | 20 | ''' 21 | TODO: Replace obstacle box w/ the box specs in your workspace: 22 | [x, y, z, r, p, y, sx, sy, sz] 23 | ''' 24 | boxes = np.array([ 25 | # obstacle 26 | # [0, 0, 0, 0, 0, 0, 0, 0, 0], 27 | [0.4, 0, 0.25, 0, 0, 0, 0.3, 0.05, 0.5], 28 | # sides 29 | [0.15, 0.46, 0.5, 0, 0, 0, 1.2, 0.01, 1.1], 30 | [0.15, -0.46, 0.5, 0, 0, 0, 1.2, 0.01, 1.1], 31 | # back 32 | [-0.41, 0, 0.5, 0, 0, 0, 0.01, 1, 1.1], 33 | # front 34 | [0.75, 0, 0.5, 0, 0, 0, 0.01, 1, 1.1], 35 | # top 36 | [0.2, 0, 1, 0, 0, 0, 1.2, 1, 0.01], 37 | # bottom 38 | [0.2, 0, -0.05, 0, 0, 0, 1.2, 1, 0.01] 39 | ]) 40 | def is_in_collision(joints): 41 | for box in boxes: 42 | if fr.check_box_collision(joints, box): 43 | return True 44 | return False 45 | 46 | desired_ee_rp = fr.ee(fr.home_joints)[3:5] 47 | def ee_upright_constraint(q): 48 | ''' 49 | TODO: Implement constraint function and its gradient. 50 | 51 | This constraint should enforce the end-effector stays upright. 52 | Hint: Use the roll and pitch angle in desired_ee_rp. The end-effector is upright in its home state. 53 | 54 | Input: 55 | q - a joint configuration 56 | 57 | Output: 58 | err - a non-negative scalar that is 0 when the constraint is satisfied 59 | grad - a vector of length 6, where the ith element is the derivative of err w.r.t. the ith element of ee 60 | ''' 61 | ee = fr.ee(q) 62 | err = np.sum((np.asarray(desired_ee_rp) - np.asarray(ee[3:5]))**2) 63 | grad = np.asarray([0, 0, 0, 2*(ee[3]-desired_ee_rp[0]), 2*(ee[4]-desired_ee_rp[1]), 0]) 64 | return err, grad 65 | 66 | ''' 67 | TODO: Fill in start and target joint positions 68 | ''' 69 | # joints_start = None 70 | # joints_target = None 71 | joints_start = fr.home_joints.copy() 72 | joints_start[0] = -np.deg2rad(45) 73 | joints_target = joints_start.copy() 74 | joints_target[0] = np.deg2rad(45) 75 | 76 | rrtc = RRTConnect(fr, is_in_collision) 77 | constraint = ee_upright_constraint 78 | plan = rrtc.plan(joints_start, joints_target, constraint) 79 | 80 | collision_boxes_publisher = CollisionBoxesPublisher('collision_boxes') 81 | rate = rospy.Rate(10) 82 | i = 0 83 | while not rospy.is_shutdown(): 84 | rate.sleep() 85 | joints = plan[i % len(plan)] 86 | fr.publish_joints(joints) 87 | fr.publish_collision_boxes(joints) 88 | collision_boxes_publisher.publish_boxes(boxes) 89 | 90 | i += 1 91 | -------------------------------------------------------------------------------- /Path Planning/visualize.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /Path Planning/writeup/PathPlanning_Pandas.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronzguan/Franka-Robot-Path-Planning/159511d01ff8392714a0675348d83c1230ab8336/Path Planning/writeup/PathPlanning_Pandas.pdf -------------------------------------------------------------------------------- /Path Planning/writeup/PathPlanning_wirteup.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronzguan/Franka-Robot-Path-Planning/159511d01ff8392714a0675348d83c1230ab8336/Path Planning/writeup/PathPlanning_wirteup.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Kinematics and Path Planning for Franka Panda Robot Arm 2 | 3 | This repo has the implementation of basic forward/inverse kinematics, and the RRT path planning for the Franka Robot Arm. 4 | 5 | * [__Kinematics__](./Kinematics): It contains the URDF & DH forward kinematics and the inverse kinematics algorithm for the Franka Robot Arm. It also has the implementation of the [**separating axis theorem**](https://www.jkh.me/files/tutorials/Separating%20Axis%20Theorem%20for%20Oriented%20Bounding%20Boxes.pdf) to avoid collision. 6 | 7 | * [__Path Planning__](./Path%20Planning): It contains the Rapidly-exploring Random Trees (RRT) for the Franka robot to reach the target pose without collision with the obstacles. It also has the implementation of the planning with constraints using Projection Sampling. 8 | 9 | This is the rViz Visualization of Franka robot, surrounding walls, and an obstacle. You will see this visualization after RRT has found a plan 10 | ![franka](images/franka.png) 11 | 12 | The Constrained RRT planning algorithm is as below: 13 | ![constrainRRT](images/ConstrainedExtend.png) 14 | 15 | * [Here](./Path%20Planning/demo) shows the demo video of both RRT and Constrained RRT for the Franka Robot in the simulation. 16 | 17 | ## Installation 18 | 19 | For Ubuntu 16.04: 20 | 21 | 1. Install ROS Kinetic on your computer following instructions here: [http://wiki.ros.org/ kinetic/Installation/Ubuntu.](http://wiki.ros.org/ kinetic/Installation/Ubuntu. ) (Make sure to install the Desktop-Full Install version of ROS Kinetic.) 22 | 23 | 2. Install libfranka and franka_ros using the following command: 24 | 25 | `sudo apt install ros-kinetic-libfranka ros-kinetic-franka-ros` 26 | 27 | 3. Install all requirements. 28 | 29 | `pip install -r requirements.txt` 30 | 31 | 4. Change the path argument in visualize.launch to the path to your working folder. -------------------------------------------------------------------------------- /images/ConstrainedExtend.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronzguan/Franka-Robot-Path-Planning/159511d01ff8392714a0675348d83c1230ab8336/images/ConstrainedExtend.png -------------------------------------------------------------------------------- /images/franka.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/aaronzguan/Franka-Robot-Path-Planning/159511d01ff8392714a0675348d83c1230ab8336/images/franka.png --------------------------------------------------------------------------------