Added Lab3 files

Author: ykarmesh

data/prm_graph.pickle

@@ -1,5 +1,9 @@
 <launch>
 	<include file="position_control.launch"/>
-	<include file="$(find realsense2_camera)/launch/rs_camera.launch"/>
+	<include file="$(find realsense2_camera)/launch/rs_camera.launch">
+        <arg name="enable_pointcloud" value="true"/>
+    	</include>
 	<include file="$(find ar_track_alvar)/launch/locobot_indiv_image.launch"/>
+    	<node type="rviz" name="rviz" pkg="rviz">
+   	</node>
 </launch>

data/prm_node.pickle

@@ -10,10 +10,10 @@
   <param
     name="use_gui"
     value="$(arg gui)" />
-  <node
+  <!--node
     name="joint_state_publisher"
     pkg="joint_state_publisher"
-    type="joint_state_publisher" />
+    type="joint_state_publisher" /-->
   <node
     name="robot_state_publisher"
     pkg="robot_state_publisher"

data/urdfinfo.npz

@@ -1,5 +1,5 @@
 import numpy as np 
-import forward_kinematics as fk
+import forward_kinematics_extrinsics as fk
 import collision_checking as cc
 from collections import defaultdict
 import time
@@ -60,12 +60,12 @@ def __init__(self, obstacles_cuboids):
         self.constraint = np.tile(np.array([-179,180])[:,np.newaxis],(1,DOF))
 
         self.obstacles_cuboids = obstacles_cuboids
-        ground_plane = np.array([[[-0.0896, 0.0004, 0.114],[0, 0, 0],[2.0, 2.0, 0.01]]])
+        ground_plane = np.array([[[0.0, 0.000, -0.10],[0, 0, 0],[2.0, 2.0, 0.01]]])
         self.obstacles_cuboids = np.append(self.obstacles_cuboids, ground_plane, axis=0)
 
         self.map = Graph()
-        self.forward_kinematics = fk.forwardKinematics()
-        self.anim = plotter.Plot()
+        self.forward_kinematics = fk.forwardKinematics( base_link='bottom_plate', end_link='gripper_link')
+        # self.anim = plotter.Plot()
 
     # Set constraint on the joint angles 
     def setConstraint(self,constraint_low, constraint_high):
@@ -91,7 +91,7 @@ def checkConstraint(self, state):
 
     # sample a node and add in the graph if its in Cfree
     def sampleNode(self):
-        #self.anim.clearPlot()
+        # self.anim.clearPlot()
         samples = np.zeros((1,DOF))
 
         # Generate Sample within Constraints
@@ -127,16 +127,17 @@ def findKNearestNeighbours(self, new_node, K):
     # Check if the sample is in collision with obstacles
     def checkPointCollision(self, sample):
         deg_to_rad = np.pi/180.
-
+        
+        np.insert(sample, 0, 0, axis=1)
         # Find arm cuboids properties in the current state
         arm_cuboids = self.forward_kinematics.getRotatedCuboid(deg_to_rad*sample)
-        #self.Animation(arm_cuboids)
+        # self.Animation(arm_cuboids)
 
 
         for i in range(arm_cuboids.shape[0]):
             for j in range(self.obstacles_cuboids.shape[0]):
                 collision = cc.collisionChecking(arm_cuboids[i,:,:], self.obstacles_cuboids[j,:,:]) 
-                #print('arm', i, 'obstacle', j, collision)
+                # print('arm', i, 'obstacle', j, collision)
 
                 # If even one of the cuboids is in collsion, return true
                 if collision is True:
@@ -157,7 +158,7 @@ def Animation(self, arm_cuboids):
             box = cc.Cuboid(self.obstacles_cuboids[i])
             self.anim.plotCuboid(box.getCorners(), i)
 
-        self.anim.showPlot(0.1)
+        self.anim.showPlot(10)
 
     # Check if there are obstacles between new_node and its nearest neighbour
     def checkLineCollision(self, new_node, nearest_node):
@@ -173,6 +174,8 @@ def checkLineCollision(self, new_node, nearest_node):
             diff = self.map.nodes[nearest_node[i,0]] - new_node.state
             # Find out the max angle difference and calculate the number of steps to move
             steps = math.ceil(abs(diff).max()/discrete_factor)
+            if steps == 0.0:
+                continue
 
             for j in range(int(steps)+1):
                 # Find the angles for the jth step
@@ -197,7 +200,7 @@ def makeGraph(self, sim_time):
         K = 5
 
         constraint = np.ones((2,5))
-        prm.setConstraint(-90*constraint[0,:],90*constraint[1,:])
+        self.setConstraint(-90*constraint[0,:],90*constraint[1,:])
 
         start_time = time.time()
 
@@ -209,7 +212,7 @@ def makeGraph(self, sim_time):
                 self.checkLineCollision(new_node, nearest_node)
 
         print(len(self.map.nodes))
-        print(self.map.adj_list)
+        #print(self.map.adj_list)
         print('Graph Successfully Built')
 
         # Save graph for future use
@@ -328,8 +331,9 @@ def loadGraph(self):
     print('Inside PRM Class')
 
     #obstacles_cuboids = np.load('../data/obstacle_cuboid.npy')
-    obstacles_cuboids = np.array([[[0.0604, 0.1854, 0.214], [0, 0, 0], [0.1, 0.125, 0.225]], \
-                                 [[0.0604, 0.1854, 0.349], [0, 0, 0], [0.125, 0.225, 0.1]]])
+    obstacles_cuboids = np.array([[[-0.12, 0, 0.1025], [0, 0, 0], [0.08, 0.19, 0.205]],
+                                  [[-0.11, 0, 0.305], [0, 0, 0], [0.07, 0.19, 0.20]],
+                                  [[-0.06, 0.0, 0.455], [0, 0, 0], [0.1, 0.15, 0.1]]])
     prm = ProbabilisticRoadMap(obstacles_cuboids)
     # prm.makeGraph(120.0)
 

external/dynamixel-workbench/dynamixel_workbench_controllers/src/position_control.cpp

@@ -1,6 +1,6 @@
 import numpy as np 
 from sklearn.preprocessing import normalize
-import forward_kinematics as fk
+import forward_kinematics_extrinsics as fk
 
 # Cuboid Class for creating cuboids from Origin, Orientation and Dimension
 class Cuboid:

launch/camera_calibration.launch

@@ -11,9 +11,10 @@ def __init__(self, base_link='bottom_plate', end_link='ar_tag', \
 
                 self.axis = None
                 self.position = None
-                # arm_cuboid_properties = np.load('../data/arm_cuboid_properties.npz')
-                # self.arm_cuboid_origin = arm_cuboid_properties[arm_cuboid_properties.files[0]]
-                # self.dimension = arm_cuboid_properties[arm_cuboid_properties.files[1]]
+                arm_cuboid_properties = np.load('../data/arm_cuboid_properties.npz')
+                self.arm_cuboid_origin = arm_cuboid_properties[arm_cuboid_properties.files[0]]
+                self.dimension = arm_cuboid_properties[arm_cuboid_properties.files[1]]
+                print(self.dimension)
                 robot = URDF.from_xml_file(path)
                 self.urdfParser(robot, base_link, end_link)
                 self.loadURDFinfo()
@@ -54,32 +55,32 @@ def getJointForwardKinematics(self, joint_angles):
                 return H
 
         # Returns the Origin, Orientation and Dimension of the collision cuboids around the robot arm links
-        # def getRotatedCuboid(self, joint_angles):
-        #         H = self.getJointForwardKinematics(joint_angles)
-        #         cuboid = np.zeros((7,3,3))
+        def getRotatedCuboid(self, joint_angles):
+                H = self.getJointForwardKinematics(joint_angles)
+                cuboid = np.zeros((7,3,3))
 
-        #         # Cuboid information for links 1 to 5 which are connected to revolute joints
-        #         for i in range(H.shape[0]):
-        #                 # Origin of the cuboid (x,y,z)
-        #                 cuboid[i,0,:] = np.matmul(H[i,:,:],self.arm_cuboid_origin[i,:].T)[0:3]
+                # Cuboid information for links 1 to 5 which are connected to revolute joints
+                for i in range(H.shape[0]-1):
+                        # Origin of the cuboid (x,y,z)
+                        cuboid[i,0,:] = np.matmul(H[i+1,:,:],self.arm_cuboid_origin[i,:].T)[0:3]
 
-        #                 # Rotation angles of the cuboid (roll, pitch, yaw)
-        #                 cuboid[i,1,:] = self.getEulerAngles(H[i,0:3,0:3])
+                        # Rotation angles of the cuboid (roll, pitch, yaw)
+                        cuboid[i,1,:] = self.getEulerAngles(H[i+1,0:3,0:3])
 
-        #                 # Dimension of the cuboid along (x,y,z)
-        #                 cuboid[i,2,:] = self.dimension[i,:]
+                        # Dimension of the cuboid along (x,y,z)
+                        cuboid[i,2,:] = self.dimension[i,:]
 
-        #         # Cuboid information for the first finger
-        #         cuboid[5,0,:] = np.matmul(H[4,:,:],self.arm_cuboid_origin[5,:].T)[0:3]
-        #         cuboid[5,1,:] = self.getEulerAngles(H[4,0:3,0:3])
-        #         cuboid[5,2,:] = self.dimension[5,:]
+                # Cuboid information for the first finger
+                cuboid[i+1,0,:] = np.matmul(H[i+1,:,:],self.arm_cuboid_origin[i+1,:].T)[0:3]
+                cuboid[i+1,1,:] = self.getEulerAngles(H[i+1,0:3,0:3])
+                cuboid[i+1,2,:] = self.dimension[i+1,:]
 
-        #         # Cuboid information for the second finger
-        #         cuboid[6,0,:] = np.matmul(H[4,:,:],self.arm_cuboid_origin[6,:].T)[0:3]
-        #         cuboid[6,1,:] = self.getEulerAngles(H[4,0:3,0:3])
-        #         cuboid[6,2,:] = self.dimension[6,:]
+                # Cuboid information for the second finger
+                cuboid[i+2,0,:] = np.matmul(H[i+1,:,:],self.arm_cuboid_origin[i+2,:].T)[0:3]
+                cuboid[i+2,1,:] = self.getEulerAngles(H[i+1,0:3,0:3])
+                cuboid[i+2,2,:] = self.dimension[i+2,:]
 
-        #         return(cuboid)
+                return(cuboid)
 
         # Checks if a matrix is a valid rotation matrix.
         @staticmethod
@@ -143,8 +144,10 @@ def loadURDFinfo(self):
 	# joint_angles = calibration_info[calibration_info.files[3]][0].reshape((1,5))
 	# fk = forwardKinematics()
         # M = fk.getJointForwardKinematics(joint_angles)
-        X = np.array([[0., 90., 0., -90., 90., 0., 0.]])
-        fk = forwardKinematics()
+        X = np.array([[0., 0., 60., 0., 0., 0.]])
+        fk = forwardKinematics(base_link='bottom_plate', end_link='gripper_link')
         M = fk.getJointForwardKinematics(X*np.pi/180)
-        for i in range(M.shape[0]):
-                print(np.matmul(np.linalg.inv(M[0,:,:]),M[i,:,:]))
+        print(M)
+        # for i in range(M.shape[0]):
+        #         print(np.matmul(np.linalg.inv(M[0,:,:]),M[i,:,:]))
+        print(fk.getRotatedCuboid(X))