generic IK controller

default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py

@@ -0,0 +1,85 @@
+# Copyright 2020 Simon Steinmann
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import sys
+import numpy as np
+from controller import Supervisor
+
+# import custom scripts. Located in the same directory as this controller
+from get_relative_position import RelativePositions
+from ik_module import inverseKinematics
+from spawn_target import spawnTarget
+
+# ----------------------------------------------------------
+# CONFIGURATION
+# ----------------------------------------------------------
+# how many simulationsteps before calculating the next IK solution. This 
+# is only relevant, if the target is constantly changing, as no new IK
+# solution gets calculated, if the target did not change.
+IKstepSize = 10 
+
+# ----------------------------------------------------------
+# ----------------------------------------------------------
+
+# Initialize the Webots Supervisor.
+supervisor = Supervisor()
+if not supervisor.getSupervisor():
+    sys.exit('WARNING: Your robot is not a supervisor! Set the supervisor field to True and restart the controller.')
+
+timeStep = int(supervisor.getBasicTimeStep())
+# Initialize our inverse kinematics module
+ik = inverseKinematics(supervisor)
+
+# check if our world already has the TARGET node. If not, we spawn it.
+target = supervisor.getFromDef('TARGET')
+try:
+    target.getPosition()
+except:
+    print('No TARGET defined. Spawning TARGET sphere')
+    spawnTarget(supervisor)
+
+
+# Initialize the RelativePositions module
+RelPos = RelativePositions(supervisor)
+
+# Initialize the arm motors and sensors. 
+n = supervisor.getNumberOfDevices()
+motors = []
+sensors = []
+for i in range(n):
+    device = supervisor.getDeviceByIndex(i)
+    #print(device.getName(), '   - NodeType:', device.getNodeType())
+    if device.getNodeType() == 54:
+        motors.append(device)
+        sensors.append(device.getPositionSensor())
+        sensors[-1].enable(timeStep)       
+
+
+target_pos_old = np.zeros((3))
+target_rot_old = np.zeros((3,3))
+print('-------------------------------------------------------')
+print('Move or rotate the TARGET sphere to move the arm...')
+while supervisor.step(IKstepSize*timeStep) != -1:
+    # Get the target position relative to the arm
+    target_pos, target_rot = RelPos.get_pos('TARGET')    
+    # check if our TARGET position or rotation has changed. If not, skip ik calculations (computationally intensive)
+    if not np.array_equal(target_pos, target_pos_old) or not np.array_equal(target_rot, target_rot_old):
+        # Call the ik_module to compute the inverse kinematics of the arm.   
+        ikResults = ik.get_ik(target_pos, target_rot)
+        # set the motor positions to the calculated ik solution
+        for i in range(len(motors)):
+            motors[i].setPosition(ikResults[i + 1])
+    target_pos_old = target_pos
+    target_rot_old = target_rot
+

default/controllers/generic_inverse_kinematic/get_relative_position.py

@@ -0,0 +1,35 @@
+import numpy as np
+
+class RelativePositions():
+    def __init__(self, Supervisor):
+        self.robot = Supervisor
+
+    def get_pos(self, DEF_target):
+        base = self.robot.getSelf()
+        target = self.robot.getFromDef(DEF_target)
+        # Get the transposed rotation matrix of the base, so we can calculate poses of
+        # everything relative to it.
+        # Get orientation of the Node we want as our new reference frame and turn it into
+        # a numpy array. Returns 1-dim list of len=9.
+        rot_base = np.array(base.getOrientation())
+        # reshape into a 3x3 rotation matrix
+        rot_base = rot_base.reshape(3, 3)
+        # Transpose the matrix, because we need world relative to the base, not the
+        # base relative to world.
+        rot_base = np.transpose(rot_base)
+        # Get the translation between the base and the world (basically where the origin
+        # of our new relative frame is).
+        # No need to use the reverse vector, as we will subtract instead of add it later.
+        pos_base = np.array(base.getPosition())
+
+
+        # target position relative to world.
+        target_pos_world = np.array(target.getPosition())
+        # Calculate the relative translation between the target and the base.
+        target_pos_world = np.subtract(target_pos_world, pos_base)
+        # Matrix multiplication with rotation matrix: target posistion relative to base.
+        target_pos_base = np.dot(rot_base, target_pos_world)
+
+        # Calculate the orientation of the target, relative to the base, all in one line.
+        target_rot_base = np.dot(rot_base, np.array(target.getOrientation()).reshape(3, 3))
+        return target_pos_base, target_rot_base

default/controllers/generic_inverse_kinematic/ik_module.py

@@ -0,0 +1,115 @@
+# Copyright 2020 Simon Steinmann
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+import sys
+import tempfile
+import numpy as np
+
+try:
+    from scipy.spatial.transform import Rotation as R
+except ImportError:
+    sys.exit('The "scipy" Python module is not installed. '
+             'To run this sample, please upgrade "pip" and install scipy with this command: "pip install scipy"')
+
+try:
+    import ikpy
+except ImportError:
+    sys.exit('The "ikpy" Python module is not installed. '
+             'To run this sample, please upgrade "pip" and install ikpy with this command: "pip install ikpy"')
+
+if ikpy.__version__[0] < '3':
+    sys.exit('The "ikpy" Python module version is too old. '
+             'Please upgrade "ikpy" Python module to version "3.0" or newer with this command: "pip install --upgrade ikpy"')
+
+class inverseKinematics():
+    def __init__(self, supervisor):
+        # Initialize the Webots Supervisor.
+        self.supervisor = supervisor
+        self.timeStep = int(self.supervisor.getBasicTimeStep())
+
+        # Get names for the arm's motors and sensors. Needed for correct armChain configuration
+        n = self.supervisor.getNumberOfDevices()
+        self.motor_names = []
+        self.sensor_names = []
+        for i in range(n):
+            device = self.supervisor.getDeviceByIndex(i)
+            if device.getNodeType() == 54:
+                sensor = device.getPositionSensor()
+                self.motor_names.append(device.getName())
+                self.sensor_names.append(sensor.getName())
+
+
+        # Create the arm chain.
+        filename = None
+        with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file:
+            filename = file.name
+            file.write(self.supervisor.getUrdf().encode('utf-8'))
+        self.armChain = ikpy.chain.Chain.from_urdf_file(filename)
+
+        # make sure only links with a motor are active for IK calculations
+        active_links = [False] * len(self.armChain.links)
+        for i in range(len(self.armChain.links)):  
+            link_name = self.armChain.links[i].name
+            active_links[i] = link_name in self.motor_names or link_name in self.sensor_names
+            if active_links[i]:
+                # ikpy includes the bounds as valid, In Webots they have to be less than the limit
+                new_lower = self.armChain.links[i].bounds[0] + 0.0000001
+                new_upper = self.armChain.links[i].bounds[1] - 0.0000001       
+                self.armChain.links[i].bounds = (new_lower, new_upper)            
+        self.armChain.active_links_mask = active_links    
+
+        # ----------------------------------------------------------
+        # Setup for correct orientation calculations
+        # ----------------------------------------------------------
+
+        # convert urdf rpy of last link into rotation matrix
+        r = R.from_euler('xyz', self.armChain.links[-1].orientation, degrees=False).as_matrix()
+        # get the translation vector of the last link
+        pos = self.armChain.links[-1].translation_vector
+        # calculate the final link vector using the dot product
+        self.final_link_vector = np.round(np.dot(r,pos), 2)
+
+        # which column we have to take from the rotation matrix, for our toolSlot axis
+        try:
+            self.rot_index = self.final_link_vector.tolist().index(np.sum(self.final_link_vector))
+        except:
+            self.rot_index=2
+            print('WARNING!!!!!!!!!!!')
+            print('The vector of the last link is not an axis. Make sure the orientation and translation of this link are set up in a way, that either the x, y, or z axis points out of the toolSlot')
+            print('The IK solver will not solve correctly for orientation. The controller will now use the z-axis as the final link vector')
+            print('rotationi matrix:')
+            print(r)
+            print('translation vector: ', pos)
+            print('final link vector:', self.final_link_vector)   
+
+        # define for ikpy, which axis we are using for the last link
+        self.toolSlot_axis = ['X', 'Y', 'Z'][self.rot_index]
+
+    def get_ik(self, target_pos, target_rot=None):   
+        # check if a target_rot is requested (3x3 numpy array)
+        if np.sum(target_rot) == None:
+            # Call "ikpy" to compute the inverse kinematics of the arm WITHOUT orientation
+            return self.armChain.inverse_kinematics(target_pos)
+        else:
+            # get the rotation vector from the target_rot rotation matrix, depending on which axis points out of the toolSlot
+            rot_vector = [target_rot[0,self.rot_index],target_rot[1,self.rot_index],target_rot[2,self.rot_index]]
+
+            # Call "ikpy" to compute the inverse kinematics of the arm WITH orientation   
+            ikResults = self.armChain.inverse_kinematics(target_pos, target_orientation=rot_vector, orientation_mode=self.toolSlot_axis)
+
+            # if ikResults is all zeros, we are dealing with a singularity
+            if sum(ikResults) == 0: 
+                # tiny change in the orientation vector to avoid singularity
+                rot_vector_new = np.array(rot_vector) + np.full((1,3), 0.0000001) 
+                ikResults = self.armChain.inverse_kinematics(target_pos, target_orientation=rot_vector_new, orientation_mode=toolSlot_axis)
+            return ikResults

default/controllers/generic_inverse_kinematic/spawn_target.py

@@ -0,0 +1,72 @@
+# Copyright 2020 Simon Steinmann
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+import os
+from shutil import copyfile
+
+class spawnTarget():
+    def __init__(self, supervisor):
+        targetSphereString = """
+        DEF TARGET Solid {
+        translation 0.68 0.4 0.69
+        rotation 0.5773519358512958 -0.5773469358518515 -0.5773519358512958 2.0944
+        scale 0.4 0.4 0.4
+        children [
+            Shape {
+            appearance PBRAppearance {
+                baseColorMap ImageTexture {
+                url [
+                    "textures/target.png"
+                ]
+                }
+                roughnessMap ImageTexture {
+                url [
+                    "textures/target.png"
+                ]
+                }
+                metalnessMap ImageTexture {
+                url [
+                    "textures/target.png"
+                ]
+                }
+                emissiveColorMap ImageTexture {
+                url [
+                    "textures/target.png"
+                ]
+                }
+                textureTransform TextureTransform {
+                scale 2 1
+                }
+            }
+            geometry Sphere {
+                radius 0.1
+                subdivision 2
+            }
+            }
+        ]
+        }
+        """
+        # copy the target.png texture into our wold
+        filePath = supervisor.getWorldPath()
+        fileNameLength = len(filePath.split('/')[-1])
+        worldPath = filePath[:-fileNameLength]        
+        controllerPath = os.path.dirname(os.path.abspath(__file__))
+        worldPath = worldPath + 'textures/'
+        if not os.path.exists(worldPath):
+            os.makedirs(worldPath)
+        copyfile(controllerPath + '/textures/target.png', worldPath + 'target.png')
+
+        # spawn the TARGET node
+        root = supervisor.getRoot()
+        rootChildren = root.getField('children')
+        rootChildren.importMFNodeFromString(rootChildren.getCount(), targetSphereString)

robots/kuka/kr5/worlds/.kuka__kr5_inverse_kinematics.wbproj

@@ -0,0 +1,9 @@
+Webots Project File version R2020b
+perspectives: 000000ff00000000fd00000003000000000000021a000003d5fc0100000002fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f007700000000000000021a0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000026b000002fafc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002fa0000008900ffffff000000030000073d000000d9fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000073d0000006900ffffff000004d0000002fa00000001000000020000000100000008fc00000000
+simulationViewPerspectives: 000000ff0000000100000002000001000000051f0100000002010000000100
+sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000200
+maximizedDockId: -1
+centralWidgetVisible: 1
+orthographicViewHeight: 1
+textFiles: 2 "../../../../webots/webots/projects/robots/abb/irb/protos/Irb4600-40.proto" "controllers/webots_IK_controller2/webots_IK_controller2.py" "../../../default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py"
+consoles: Console:All:All

robots/kuka/kr5/worlds/kuka__kr5_inverse_kinematics.wbt

@@ -0,0 +1,60 @@
+#VRML_SIM R2020b utf8
+WorldInfo {
+  basicTimeStep 16
+  coordinateSystem "NUE"
+}
+Viewpoint {
+  orientation -0.9944585328663142 -0.10284538304268112 -0.021795724263762997 0.7597448038022628
+  position -0.080546513752037 2.5616312288532073 2.6301726976621063
+}
+TexturedBackground {
+}
+TexturedBackgroundLight {
+}
+RectangleArena {
+  floorSize 3 3
+  floorTileSize 0.25 0.25
+  wallHeight 0.05
+}
+DEF TARGET Solid {
+  translation 0.26 0.4 0.42
+  rotation -1 0 0 -1.5708053071795867
+  scale 0.4 0.4 0.4
+  children [
+    Shape {
+      appearance PBRAppearance {
+        baseColorMap ImageTexture {
+          url [
+            "textures/target.png"
+          ]
+        }
+        roughnessMap ImageTexture {
+          url [
+            "textures/target.png"
+          ]
+        }
+        metalnessMap ImageTexture {
+          url [
+            "textures/target.png"
+          ]
+        }
+        emissiveColorMap ImageTexture {
+          url [
+            "textures/target.png"
+          ]
+        }
+        textureTransform TextureTransform {
+          scale 2 1
+        }
+      }
+      geometry Sphere {
+        radius 0.1
+        subdivision 2
+      }
+    }
+  ]
+}
+KukaKr5Arc {
+  controller "generic_inverse_kinematic"
+  supervisor TRUE
+}

robots/kuka/lbr_iiwa/worlds/.kuka_inverse_kinematics.wbproj

@@ -0,0 +1,9 @@
+Webots Project File version R2020b
+perspectives: 000000ff00000000fd00000003000000000000021a000003d5fc0100000002fb0000001e00480074006d006c0052006f0062006f007400570069006e0064006f007700000000000000021a0000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000006900ffffff000000010000026b000002fafc0200000001fb0000001400540065007800740045006400690074006f00720100000016000002fa0000008900ffffff000000030000073d000000d9fc0100000001fb0000001a0043006f006e0073006f006c00650041006c006c0041006c006c01000000000000073d0000006900ffffff000004d0000002fa00000001000000020000000100000008fc00000000
+simulationViewPerspectives: 000000ff0000000100000002000001000000051f0100000002010000000100
+sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000200
+maximizedDockId: -1
+centralWidgetVisible: 1
+orthographicViewHeight: 1
+textFiles: 0 "../../../../webots/webots/projects/robots/abb/irb/protos/Irb4600-40.proto"
+consoles: Console:All:All