From 9cab185406e52971e0cdad41d18d129a8e81fe3e Mon Sep 17 00:00:00 2001 From: Simon Steinmann <52213164+Simon-Steinmann@users.noreply.github.com> Date: Tue, 18 Aug 2020 16:27:38 +0200 Subject: [PATCH 01/12] Create dummy_file.txt --- dummy_file.txt | 1 + 1 file changed, 1 insertion(+) create mode 100644 dummy_file.txt diff --git a/dummy_file.txt b/dummy_file.txt new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/dummy_file.txt @@ -0,0 +1 @@ + From 9beada82896642a3494a3189861a8d5db1d80926 Mon Sep 17 00:00:00 2001 From: Simon Steinmann <52213164+Simon-Steinmann@users.noreply.github.com> Date: Tue, 18 Aug 2020 16:27:59 +0200 Subject: [PATCH 02/12] Add files via upload --- .../get_relative_position.cpython-37.pyc | Bin 0 -> 1002 bytes .../generic_inverse_kinematic.py | 151 ++++++++++++++++++ .../get_relative_position.py | 36 +++++ 3 files changed, 187 insertions(+) create mode 100644 default/controllers/generic_inverse_kinematic/__pycache__/get_relative_position.cpython-37.pyc create mode 100644 default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py create mode 100644 default/controllers/generic_inverse_kinematic/get_relative_position.py diff --git a/default/controllers/generic_inverse_kinematic/__pycache__/get_relative_position.cpython-37.pyc b/default/controllers/generic_inverse_kinematic/__pycache__/get_relative_position.cpython-37.pyc new file mode 100644 index 0000000000000000000000000000000000000000..548146bbbabb3bfcbdafe56c3006054f70d1a005 GIT binary patch literal 1002 zcmZuv!EV$r5Vaj=H=Aw?T#%>`_dU?PBZR6_%56dPmW!3fUYe4`!FGzaN>7wO-~)C~ zh!5areC5<%;KWR_L6wRnkKfF^8PAM;w7VMr%~aUg~O|>9TC9+^wG1MqjAR*|}89v?(08Dra<2C}ZazRi#XxiM+Z5Rf$h| zrAjC<1CJACTdt_L^&ZqK9|>URPKXdo?dqpj`}aCK7-;`moB)3eyKC_~fRwIM_xj$7W2 z0FQgT<#ySJ%;1gMaT_fe44W-(@jJ|R3?6nfBt7;#n#-uC12j+Js zSCxaQ>B^6dvgc{7{KT2GvM@O1cYrgzxQ}de2CmHcK*D=Y42Nry`1s9N=VvJd zje0GKjCKMoNd(Dy^sx8gvxqO+6f${m$I_7p!w}tlkzU{%bbVob1d4qOg3y>w!#y?) zV!p@W1oVtvJ-Pp|P$NbzW+DECDlL@|J{F?XvMCTxh4|d0Mei|qgTWi_lz{)$U>yek l4^F_Mh-egamRaD#swwN0nGU4?Mf>Pwf9WfVDKJ+Q`~_Ai4LAS* literal 0 HcmV?d00001 diff --git a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py new file mode 100644 index 0000000..dd6366c --- /dev/null +++ b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py @@ -0,0 +1,151 @@ +# 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. + +"""Demonstration of inverse kinematics using the "ikpy" Python module.""" + +import sys +import tempfile +from math import sin, pi +import math +import numpy as np +from controller import Supervisor +from get_relative_position import RelativePositions +from scipy.spatial.transform import Rotation as R + +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"') + + +# ---------------------------------------------------------- +# 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() +timeStep = int(supervisor.getBasicTimeStep()) + +# Initialize the RelativePositions class +RelPos = RelativePositions(supervisor) + +# Initialize the arm motors and sensors. +n = supervisor.getNumberOfDevices() +motors = [] +sensors = [] +motor_names = [] +sensor_names = [] +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()) + motor_names.append(device.getName()) + sensor_names.append(sensors[-1].getName()) + sensors[-1].enable(timeStep) + + + + +# Create the arm chain. +filename = None +with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file: + filename = file.name + file.write(supervisor.getUrdf().encode('utf-8')) +armChain = ikpy.chain.Chain.from_urdf_file(filename) + + + + +# make sure only links with a motor are active for IK calculations +active_links = [False] * len(armChain.links) + +for i in range(len(armChain.links)): + link_name = armChain.links[i].name + active_links[i] = link_name in motor_names or link_name in sensor_names + if active_links[i]: + # ikpy includes the bounds as valid, In Webots they have to be less than the limit + new_lower = armChain.links[i].bounds[0] + 0.0000001 + new_upper = armChain.links[i].bounds[1] - 0.0000001 + armChain.links[i].bounds = (new_lower, new_upper) + +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', armChain.links[-1].orientation, degrees=False).as_matrix() +# get the translation vector of the last link +pos = armChain.links[-1].translation_vector +# calculate the final link vector using the dot product +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: + rot_index = final_link_vector.tolist().index(np.sum(final_link_vector)) +except: + 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:', final_link_vector) + +# define for ikpy, which axis we are using +toolSlot_axis = ['X', 'Y', 'Z'][rot_index] + + +# ---------------------------------------------------------- +# Main loop, following the target sphere +# ---------------------------------------------------------- + +target_pos_old = np.zeros((3)) +target_rot_old = [0]*3 +print('Move the yellow and black 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') + + # get the rotation vector from the target_rot rotation matrix, depending on which axis points out of the toolSlot + rot_vector = [target_rot[0,rot_index],target_rot[1,rot_index],target_rot[2,rot_index]] + + # Call "ikpy" to compute the inverse kinematics of the arm. + if any(target_pos != target_pos_old) or rot_vector != target_rot_old: + ikResults = armChain.inverse_kinematics(target_pos, target_orientation=rot_vector, orientation_mode=toolSlot_axis) + if sum(ikResults) == 0: # dealing with singularity + rot_vector_new = np.array(rot_vector) + np.full((1,3), 0.0000001) # tiny change in the orientation vector to avoid singularity + ikResults = armChain.inverse_kinematics(target_pos, target_orientation=rot_vector_new, orientation_mode=toolSlot_axis) + for i in range(len(motors)): + motors[i].setPosition(ikResults[i + 1]) + target_pos_old = target_pos + target_rot_old = rot_vector + diff --git a/default/controllers/generic_inverse_kinematic/get_relative_position.py b/default/controllers/generic_inverse_kinematic/get_relative_position.py new file mode 100644 index 0000000..e9f3cc4 --- /dev/null +++ b/default/controllers/generic_inverse_kinematic/get_relative_position.py @@ -0,0 +1,36 @@ +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 \ No newline at end of file From 405ff8710dba755279073f92f16f1936f7ce261a Mon Sep 17 00:00:00 2001 From: Simon Steinmann <52213164+Simon-Steinmann@users.noreply.github.com> Date: Tue, 18 Aug 2020 16:28:18 +0200 Subject: [PATCH 03/12] Delete dummy_file.txt --- dummy_file.txt | 1 - 1 file changed, 1 deletion(-) delete mode 100644 dummy_file.txt diff --git a/dummy_file.txt b/dummy_file.txt deleted file mode 100644 index 8b13789..0000000 --- a/dummy_file.txt +++ /dev/null @@ -1 +0,0 @@ - From bff777fd4865dfca264bfcf92a53e4fef82ebe49 Mon Sep 17 00:00:00 2001 From: Simon Steinmann <52213164+Simon-Steinmann@users.noreply.github.com> Date: Tue, 18 Aug 2020 17:06:48 +0200 Subject: [PATCH 04/12] add kr5 example world --- robots/kuka/kr5/protos/KukaKr5Arc.proto | 24 +++---- .../worlds/kuka__kr5_inverse_kinematics.wbt | 60 ++++++++++++++++++ robots/kuka/kr5/worlds/textures/target.png | Bin 0 -> 230 bytes 3 files changed, 72 insertions(+), 12 deletions(-) create mode 100644 robots/kuka/kr5/worlds/kuka__kr5_inverse_kinematics.wbt create mode 100644 robots/kuka/kr5/worlds/textures/target.png diff --git a/robots/kuka/kr5/protos/KukaKr5Arc.proto b/robots/kuka/kr5/protos/KukaKr5Arc.proto index 92d480f..d500469 100644 --- a/robots/kuka/kr5/protos/KukaKr5Arc.proto +++ b/robots/kuka/kr5/protos/KukaKr5Arc.proto @@ -46,8 +46,8 @@ PROTO KukaKr5Arc [ RotationalMotor { name "joint_a1" maxVelocity 2.68780704807 - minPosition -2.70526034059 - maxPosition 2.70526034059 + minPosition -9.70526034059 + maxPosition 9.70526034059 maxTorque 10000 } PositionSensor { @@ -83,8 +83,8 @@ PROTO KukaKr5Arc [ RotationalMotor { name "joint_a2" maxVelocity 2.68780704807 - minPosition -3.14159265359 - maxPosition 1.1344640138 + minPosition -9.14159265359 + maxPosition 9.1344640138 maxTorque 10000 } PositionSensor { @@ -118,8 +118,8 @@ PROTO KukaKr5Arc [ RotationalMotor { name "joint_a3" maxVelocity 3.97935069455 - minPosition -0.261799387799 - maxPosition 2.75762021815 + minPosition -9.261799387799 + maxPosition 9 maxTorque 10000 } PositionSensor { @@ -149,8 +149,8 @@ PROTO KukaKr5Arc [ RotationalMotor { name "joint_a4" maxVelocity 5.98647933434 - minPosition -6.10865238198 - maxPosition 6.10865238198 + minPosition -9.10865238198 + maxPosition 9.10865238198 maxTorque 10000 } PositionSensor { @@ -176,8 +176,8 @@ PROTO KukaKr5Arc [ RotationalMotor { name "joint_a5" maxVelocity 6.70206432766 - minPosition -2.26892802759 - maxPosition 2.26892802759 + minPosition -9.26892802759 + maxPosition 9.26892802759 maxTorque 10000 } PositionSensor { @@ -201,8 +201,8 @@ PROTO KukaKr5Arc [ RotationalMotor { name "joint_a6" maxVelocity 12.5838239069 - minPosition -6.10865238198 - maxPosition 6.10865238198 + minPosition -9.10865238198 + maxPosition 9 maxTorque 10000 } PositionSensor { diff --git a/robots/kuka/kr5/worlds/kuka__kr5_inverse_kinematics.wbt b/robots/kuka/kr5/worlds/kuka__kr5_inverse_kinematics.wbt new file mode 100644 index 0000000..645740e --- /dev/null +++ b/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 +} diff --git a/robots/kuka/kr5/worlds/textures/target.png b/robots/kuka/kr5/worlds/textures/target.png new file mode 100644 index 0000000000000000000000000000000000000000..ae126662c24edf49122e9a0cd9415fde0187924f GIT binary patch literal 230 zcmeAS@N?(olHy`uVBq!ia0vp^4j|0I1SD0tpLGH$&H|6fVg?31We{epSZZGe6l5>) z^mS!_%*?~ZBv734@d{8#vcxr_#5q4VH#M(>!MP|ku_QG`p**uBL&4qCHz2%`PaLQy z!PCVtB;(%O8-~0L3Oud{#rR`CuJ0BVUZK(WWL|^X^!=ubckWGp{a0{4J5V(yDA@nz j-~Di1ZcG`F$eyc=%Tid2o2Qo@193fF{an^LB{Ts5w&6K9 literal 0 HcmV?d00001 From e732ad820d4b0a852d522117efe14a13e8b14379 Mon Sep 17 00:00:00 2001 From: Simon Steinmann <52213164+Simon-Steinmann@users.noreply.github.com> Date: Tue, 18 Aug 2020 17:11:04 +0200 Subject: [PATCH 05/12] add lbr_iiwa world --- .../lrb_iiwa/protos/KukaLbrIiwa14R820.proto | 4 +- .../worlds/kuka_inverse_kinematics.wbt | 62 ++++++++++++++++++ .../kuka/lrb_iiwa/worlds/textures/target.png | Bin 0 -> 230 bytes 3 files changed, 64 insertions(+), 2 deletions(-) create mode 100644 robots/kuka/lrb_iiwa/worlds/kuka_inverse_kinematics.wbt create mode 100644 robots/kuka/lrb_iiwa/worlds/textures/target.png diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820.proto b/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820.proto index a60db28..5fc9cec 100644 --- a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820.proto +++ b/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820.proto @@ -223,8 +223,8 @@ PROTO KukaLbrIiwa14R820 [ KukaLbrIiwa14R820_link_7_2Mesh { } Solid { - translation 0.000000 0.000000 0.126000 - rotation 0.000000 1.000000 0.000000 0.000000 + translation 0.000000 0.000000 0.126000 + rotation 0.000000 1.000000 0.000000 0 children [ Group { children IS toolSlot diff --git a/robots/kuka/lrb_iiwa/worlds/kuka_inverse_kinematics.wbt b/robots/kuka/lrb_iiwa/worlds/kuka_inverse_kinematics.wbt new file mode 100644 index 0000000..afe70c9 --- /dev/null +++ b/robots/kuka/lrb_iiwa/worlds/kuka_inverse_kinematics.wbt @@ -0,0 +1,62 @@ +#VRML_SIM R2020b utf8 +WorldInfo { + basicTimeStep 16 + coordinateSystem "NUE" +} +Viewpoint { + orientation -0.4662465401337797 0.8750484017450887 0.1300171466255336 0.4908980536393356 + position 1.3725010340850807 1.1474384464970784 3.1002260652489135 +} +TexturedBackground { +} +TexturedBackgroundLight { +} +RectangleArena { + floorSize 3 3 + floorTileSize 0.25 0.25 + wallHeight 0.05 +} +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 + } + } + ] +} +Irb4600-40 { + rotation 1 0 0 -1.5707953071795862 + controller "generic_inverse_kinematic" + supervisor TRUE + staticBase TRUE +} diff --git a/robots/kuka/lrb_iiwa/worlds/textures/target.png b/robots/kuka/lrb_iiwa/worlds/textures/target.png new file mode 100644 index 0000000000000000000000000000000000000000..ae126662c24edf49122e9a0cd9415fde0187924f GIT binary patch literal 230 zcmeAS@N?(olHy`uVBq!ia0vp^4j|0I1SD0tpLGH$&H|6fVg?31We{epSZZGe6l5>) z^mS!_%*?~ZBv734@d{8#vcxr_#5q4VH#M(>!MP|ku_QG`p**uBL&4qCHz2%`PaLQy z!PCVtB;(%O8-~0L3Oud{#rR`CuJ0BVUZK(WWL|^X^!=ubckWGp{a0{4J5V(yDA@nz j-~Di1ZcG`F$eyc=%Tid2o2Qo@193fF{an^LB{Ts5w&6K9 literal 0 HcmV?d00001 From ae48bced546b9acafa5c9b3a097175d261bfc40e Mon Sep 17 00:00:00 2001 From: Simon Steinmann Date: Tue, 18 Aug 2020 17:37:00 +0200 Subject: [PATCH 06/12] clean upload --- robots/kuka/kr5/protos/KukaKr5Arc.proto | 24 +++++++++--------- .../.kuka__kr5_inverse_kinematics.wbproj | 9 +++++++ .../protos/KukaLbrIiwa14R820.proto | 4 +-- .../KukaLbrIiwa14R820_base_link_0Mesh.proto | 0 .../KukaLbrIiwa14R820_link_1_0Mesh.proto | 0 .../KukaLbrIiwa14R820_link_2_0Mesh.proto | 0 .../KukaLbrIiwa14R820_link_2_1Mesh.proto | 0 .../KukaLbrIiwa14R820_link_3_0Mesh.proto | 0 .../KukaLbrIiwa14R820_link_3_1Mesh.proto | 0 .../KukaLbrIiwa14R820_link_3_2Mesh.proto | 0 .../KukaLbrIiwa14R820_link_4_1Mesh.proto | 0 .../KukaLbrIiwa14R820_link_4_2Mesh.proto | 0 .../KukaLbrIiwa14R820_link_5_0Mesh.proto | 0 .../KukaLbrIiwa14R820_link_5_1Mesh.proto | 0 .../KukaLbrIiwa14R820_link_5_2Mesh.proto | 0 .../KukaLbrIiwa14R820_link_6_0Mesh.proto | 0 .../KukaLbrIiwa14R820_link_6_1Mesh.proto | 0 .../KukaLbrIiwa14R820_link_7_0Mesh.proto | 0 .../KukaLbrIiwa14R820_link_7_1Mesh.proto | 0 .../KukaLbrIiwa14R820_link_7_2Mesh.proto | 0 .../worlds/.kuka_inverse_kinematics.wbproj | 9 +++++++ .../worlds/kuka_inverse_kinematics.wbt | 0 .../worlds/textures/target.png | Bin 23 files changed, 32 insertions(+), 14 deletions(-) create mode 100644 robots/kuka/kr5/worlds/.kuka__kr5_inverse_kinematics.wbproj rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820.proto (99%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_base_link_0Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_1_0Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_2_0Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_2_1Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_0Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_1Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_2Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_4_1Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_4_2Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_0Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_1Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_2Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_6_0Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_6_1Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_0Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_1Mesh.proto (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_2Mesh.proto (100%) create mode 100644 robots/kuka/lbr_iiwa/worlds/.kuka_inverse_kinematics.wbproj rename robots/kuka/{lrb_iiwa => lbr_iiwa}/worlds/kuka_inverse_kinematics.wbt (100%) rename robots/kuka/{lrb_iiwa => lbr_iiwa}/worlds/textures/target.png (100%) diff --git a/robots/kuka/kr5/protos/KukaKr5Arc.proto b/robots/kuka/kr5/protos/KukaKr5Arc.proto index d500469..92d480f 100644 --- a/robots/kuka/kr5/protos/KukaKr5Arc.proto +++ b/robots/kuka/kr5/protos/KukaKr5Arc.proto @@ -46,8 +46,8 @@ PROTO KukaKr5Arc [ RotationalMotor { name "joint_a1" maxVelocity 2.68780704807 - minPosition -9.70526034059 - maxPosition 9.70526034059 + minPosition -2.70526034059 + maxPosition 2.70526034059 maxTorque 10000 } PositionSensor { @@ -83,8 +83,8 @@ PROTO KukaKr5Arc [ RotationalMotor { name "joint_a2" maxVelocity 2.68780704807 - minPosition -9.14159265359 - maxPosition 9.1344640138 + minPosition -3.14159265359 + maxPosition 1.1344640138 maxTorque 10000 } PositionSensor { @@ -118,8 +118,8 @@ PROTO KukaKr5Arc [ RotationalMotor { name "joint_a3" maxVelocity 3.97935069455 - minPosition -9.261799387799 - maxPosition 9 + minPosition -0.261799387799 + maxPosition 2.75762021815 maxTorque 10000 } PositionSensor { @@ -149,8 +149,8 @@ PROTO KukaKr5Arc [ RotationalMotor { name "joint_a4" maxVelocity 5.98647933434 - minPosition -9.10865238198 - maxPosition 9.10865238198 + minPosition -6.10865238198 + maxPosition 6.10865238198 maxTorque 10000 } PositionSensor { @@ -176,8 +176,8 @@ PROTO KukaKr5Arc [ RotationalMotor { name "joint_a5" maxVelocity 6.70206432766 - minPosition -9.26892802759 - maxPosition 9.26892802759 + minPosition -2.26892802759 + maxPosition 2.26892802759 maxTorque 10000 } PositionSensor { @@ -201,8 +201,8 @@ PROTO KukaKr5Arc [ RotationalMotor { name "joint_a6" maxVelocity 12.5838239069 - minPosition -9.10865238198 - maxPosition 9 + minPosition -6.10865238198 + maxPosition 6.10865238198 maxTorque 10000 } PositionSensor { diff --git a/robots/kuka/kr5/worlds/.kuka__kr5_inverse_kinematics.wbproj b/robots/kuka/kr5/worlds/.kuka__kr5_inverse_kinematics.wbproj new file mode 100644 index 0000000..0e0e0d3 --- /dev/null +++ b/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 diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820.proto similarity index 99% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820.proto index 5fc9cec..a60db28 100644 --- a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820.proto +++ b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820.proto @@ -223,8 +223,8 @@ PROTO KukaLbrIiwa14R820 [ KukaLbrIiwa14R820_link_7_2Mesh { } Solid { - translation 0.000000 0.000000 0.126000 - rotation 0.000000 1.000000 0.000000 0 + translation 0.000000 0.000000 0.126000 + rotation 0.000000 1.000000 0.000000 0.000000 children [ Group { children IS toolSlot diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_base_link_0Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_base_link_0Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_base_link_0Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_base_link_0Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_1_0Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_1_0Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_1_0Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_1_0Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_2_0Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_2_0Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_2_0Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_2_0Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_2_1Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_2_1Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_2_1Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_2_1Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_0Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_0Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_0Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_0Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_1Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_1Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_1Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_1Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_2Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_2Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_2Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_3_2Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_4_1Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_4_1Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_4_1Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_4_1Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_4_2Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_4_2Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_4_2Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_4_2Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_0Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_0Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_0Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_0Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_1Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_1Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_1Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_1Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_2Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_2Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_2Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_5_2Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_6_0Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_6_0Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_6_0Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_6_0Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_6_1Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_6_1Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_6_1Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_6_1Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_0Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_0Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_0Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_0Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_1Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_1Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_1Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_1Mesh.proto diff --git a/robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_2Mesh.proto b/robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_2Mesh.proto similarity index 100% rename from robots/kuka/lrb_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_2Mesh.proto rename to robots/kuka/lbr_iiwa/protos/KukaLbrIiwa14R820_meshes/KukaLbrIiwa14R820_link_7_2Mesh.proto diff --git a/robots/kuka/lbr_iiwa/worlds/.kuka_inverse_kinematics.wbproj b/robots/kuka/lbr_iiwa/worlds/.kuka_inverse_kinematics.wbproj new file mode 100644 index 0000000..ec33ddf --- /dev/null +++ b/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 diff --git a/robots/kuka/lrb_iiwa/worlds/kuka_inverse_kinematics.wbt b/robots/kuka/lbr_iiwa/worlds/kuka_inverse_kinematics.wbt similarity index 100% rename from robots/kuka/lrb_iiwa/worlds/kuka_inverse_kinematics.wbt rename to robots/kuka/lbr_iiwa/worlds/kuka_inverse_kinematics.wbt diff --git a/robots/kuka/lrb_iiwa/worlds/textures/target.png b/robots/kuka/lbr_iiwa/worlds/textures/target.png similarity index 100% rename from robots/kuka/lrb_iiwa/worlds/textures/target.png rename to robots/kuka/lbr_iiwa/worlds/textures/target.png From e15373e884590cb8e35ad470101fa5c4bb110c40 Mon Sep 17 00:00:00 2001 From: Simon Steinmann Date: Tue, 18 Aug 2020 19:26:33 +0200 Subject: [PATCH 07/12] auto-spawn target and ik_module --- .../get_relative_position.cpython-37.pyc | Bin 1002 -> 0 bytes .../generic_inverse_kinematic copy.py | 202 ++++++++++++++++++ .../generic_inverse_kinematic.py | 109 +++------- .../generic_inverse_kinematic/ik_module.py | 111 ++++++++++ .../generic_inverse_kinematic/spawn_target.py | 59 +++++ 5 files changed, 399 insertions(+), 82 deletions(-) delete mode 100644 default/controllers/generic_inverse_kinematic/__pycache__/get_relative_position.cpython-37.pyc create mode 100644 default/controllers/generic_inverse_kinematic/generic_inverse_kinematic copy.py create mode 100644 default/controllers/generic_inverse_kinematic/ik_module.py create mode 100644 default/controllers/generic_inverse_kinematic/spawn_target.py diff --git a/default/controllers/generic_inverse_kinematic/__pycache__/get_relative_position.cpython-37.pyc b/default/controllers/generic_inverse_kinematic/__pycache__/get_relative_position.cpython-37.pyc deleted file mode 100644 index 548146bbbabb3bfcbdafe56c3006054f70d1a005..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 1002 zcmZuv!EV$r5Vaj=H=Aw?T#%>`_dU?PBZR6_%56dPmW!3fUYe4`!FGzaN>7wO-~)C~ zh!5areC5<%;KWR_L6wRnkKfF^8PAM;w7VMr%~aUg~O|>9TC9+^wG1MqjAR*|}89v?(08Dra<2C}ZazRi#XxiM+Z5Rf$h| zrAjC<1CJACTdt_L^&ZqK9|>URPKXdo?dqpj`}aCK7-;`moB)3eyKC_~fRwIM_xj$7W2 z0FQgT<#ySJ%;1gMaT_fe44W-(@jJ|R3?6nfBt7;#n#-uC12j+Js zSCxaQ>B^6dvgc{7{KT2GvM@O1cYrgzxQ}de2CmHcK*D=Y42Nry`1s9N=VvJd zje0GKjCKMoNd(Dy^sx8gvxqO+6f${m$I_7p!w}tlkzU{%bbVob1d4qOg3y>w!#y?) zV!p@W1oVtvJ-Pp|P$NbzW+DECDlL@|J{F?XvMCTxh4|d0Mei|qgTWi_lz{)$U>yek l4^F_Mh-egamRaD#swwN0nGU4?Mf>Pwf9WfVDKJ+Q`~_Ai4LAS* diff --git a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic copy.py b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic copy.py new file mode 100644 index 0000000..6e830ae --- /dev/null +++ b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic copy.py @@ -0,0 +1,202 @@ +# 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. + +"""Demonstration of inverse kinematics using the "ikpy" Python module.""" + +import sys +import tempfile +from math import sin, pi +import math +import numpy as np +from controller import Supervisor +from get_relative_position import RelativePositions +from scipy.spatial.transform import Rotation as R + +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"') + + +# ---------------------------------------------------------- +# 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() +timeStep = int(supervisor.getBasicTimeStep()) + +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/checkered_marble.jpg" + ] + } + roughnessMap ImageTexture { + url [ + "textures/checkered_marble.jpg" + ] + } + metalnessMap ImageTexture { + url [ + "textures/checkered_marble.jpg" + ] + } + emissiveColorMap ImageTexture { + url [ + "textures/checkered_marble.jpg" + ] + } + textureTransform TextureTransform { + scale 2 1 + } + } + geometry Sphere { + radius 0.1 + subdivision 2 + } + } + ] +} +""" +target = supervisor.getFromDef('TARGET') +try: + target.getPosition() +except: + print('No TARGET defined. Spawning TARGET sphere') + root = supervisor.getRoot() + rootChildren = root.getField('children') + rootChildren.importMFNodeFromString(rootChildren.getCount(), targetSphereString) + + + +# Initialize the RelativePositions class +RelPos = RelativePositions(supervisor) + +# Initialize the arm motors and sensors. +n = supervisor.getNumberOfDevices() +motors = [] +sensors = [] +motor_names = [] +sensor_names = [] +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()) + motor_names.append(device.getName()) + sensor_names.append(sensors[-1].getName()) + sensors[-1].enable(timeStep) + + + + +# Create the arm chain. +filename = None +with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file: + filename = file.name + file.write(supervisor.getUrdf().encode('utf-8')) +armChain = ikpy.chain.Chain.from_urdf_file(filename) + + + + +# make sure only links with a motor are active for IK calculations +active_links = [False] * len(armChain.links) + +for i in range(len(armChain.links)): + link_name = armChain.links[i].name + active_links[i] = link_name in motor_names or link_name in sensor_names + if active_links[i]: + # ikpy includes the bounds as valid, In Webots they have to be less than the limit + new_lower = armChain.links[i].bounds[0] + 0.0000001 + new_upper = armChain.links[i].bounds[1] - 0.0000001 + armChain.links[i].bounds = (new_lower, new_upper) + +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', armChain.links[-1].orientation, degrees=False).as_matrix() +# get the translation vector of the last link +pos = armChain.links[-1].translation_vector +# calculate the final link vector using the dot product +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: + rot_index = final_link_vector.tolist().index(np.sum(final_link_vector)) +except: + 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:', final_link_vector) + +# define for ikpy, which axis we are using +toolSlot_axis = ['X', 'Y', 'Z'][rot_index] + + +# ---------------------------------------------------------- +# Main loop, following the target sphere +# ---------------------------------------------------------- + +target_pos_old = np.zeros((3)) +target_rot_old = [0]*3 +print('Move the yellow and black 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') + + # get the rotation vector from the target_rot rotation matrix, depending on which axis points out of the toolSlot + rot_vector = [target_rot[0,rot_index],target_rot[1,rot_index],target_rot[2,rot_index]] + + # Call "ikpy" to compute the inverse kinematics of the arm. + if any(target_pos != target_pos_old) or rot_vector != target_rot_old: + ikResults = armChain.inverse_kinematics(target_pos, target_orientation=rot_vector, orientation_mode=toolSlot_axis) + if sum(ikResults) == 0: # dealing with singularity + rot_vector_new = np.array(rot_vector) + np.full((1,3), 0.0000001) # tiny change in the orientation vector to avoid singularity + ikResults = armChain.inverse_kinematics(target_pos, target_orientation=rot_vector_new, orientation_mode=toolSlot_axis) + for i in range(len(motors)): + motors[i].setPosition(ikResults[i + 1]) + target_pos_old = target_pos + target_rot_old = rot_vector + diff --git a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py index dd6366c..ee2c5ef 100644 --- a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py +++ b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py @@ -16,13 +16,15 @@ import sys import tempfile -from math import sin, pi -import math import numpy as np from controller import Supervisor -from get_relative_position import RelativePositions from scipy.spatial.transform import Rotation as R +# 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 + try: import ikpy except ImportError: @@ -42,13 +44,22 @@ # solution gets calculated, if the target did not change. IKstepSize = 10 - # ---------------------------------------------------------- # ---------------------------------------------------------- # Initialize the Webots Supervisor. supervisor = Supervisor() timeStep = int(supervisor.getBasicTimeStep()) +ik = inverseKinematics(supervisor) + +target = supervisor.getFromDef('TARGET') +try: + target.getPosition() +except: + print('No TARGET defined. Spawning TARGET sphere') + spawnTarget(supervisor) + + # Initialize the RelativePositions class RelPos = RelativePositions(supervisor) @@ -57,95 +68,29 @@ n = supervisor.getNumberOfDevices() motors = [] sensors = [] -motor_names = [] -sensor_names = [] 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()) - motor_names.append(device.getName()) - sensor_names.append(sensors[-1].getName()) - sensors[-1].enable(timeStep) - + sensors[-1].enable(timeStep) - - -# Create the arm chain. -filename = None -with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file: - filename = file.name - file.write(supervisor.getUrdf().encode('utf-8')) -armChain = ikpy.chain.Chain.from_urdf_file(filename) - - - - -# make sure only links with a motor are active for IK calculations -active_links = [False] * len(armChain.links) - -for i in range(len(armChain.links)): - link_name = armChain.links[i].name - active_links[i] = link_name in motor_names or link_name in sensor_names - if active_links[i]: - # ikpy includes the bounds as valid, In Webots they have to be less than the limit - new_lower = armChain.links[i].bounds[0] + 0.0000001 - new_upper = armChain.links[i].bounds[1] - 0.0000001 - armChain.links[i].bounds = (new_lower, new_upper) - -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', armChain.links[-1].orientation, degrees=False).as_matrix() -# get the translation vector of the last link -pos = armChain.links[-1].translation_vector -# calculate the final link vector using the dot product -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: - rot_index = final_link_vector.tolist().index(np.sum(final_link_vector)) -except: - 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:', final_link_vector) - -# define for ikpy, which axis we are using -toolSlot_axis = ['X', 'Y', 'Z'][rot_index] - - -# ---------------------------------------------------------- -# Main loop, following the target sphere -# ---------------------------------------------------------- - + target_pos_old = np.zeros((3)) -target_rot_old = [0]*3 -print('Move the yellow and black sphere to move the arm...') +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') - - # get the rotation vector from the target_rot rotation matrix, depending on which axis points out of the toolSlot - rot_vector = [target_rot[0,rot_index],target_rot[1,rot_index],target_rot[2,rot_index]] - - # Call "ikpy" to compute the inverse kinematics of the arm. - if any(target_pos != target_pos_old) or rot_vector != target_rot_old: - ikResults = armChain.inverse_kinematics(target_pos, target_orientation=rot_vector, orientation_mode=toolSlot_axis) - if sum(ikResults) == 0: # dealing with singularity - rot_vector_new = np.array(rot_vector) + np.full((1,3), 0.0000001) # tiny change in the orientation vector to avoid singularity - ikResults = armChain.inverse_kinematics(target_pos, target_orientation=rot_vector_new, orientation_mode=toolSlot_axis) + 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 = rot_vector + target_rot_old = target_rot diff --git a/default/controllers/generic_inverse_kinematic/ik_module.py b/default/controllers/generic_inverse_kinematic/ik_module.py new file mode 100644 index 0000000..d1dfef1 --- /dev/null +++ b/default/controllers/generic_inverse_kinematic/ik_module.py @@ -0,0 +1,111 @@ +# 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): + if np.sum(target_rot) == None: + 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. + ikResults = self.armChain.inverse_kinematics(target_pos, target_orientation=rot_vector, orientation_mode=self.toolSlot_axis) + if sum(ikResults) == 0: # dealing with singularity + rot_vector_new = np.array(rot_vector) + np.full((1,3), 0.0000001) # tiny change in the orientation vector to avoid singularity + ikResults = self.armChain.inverse_kinematics(target_pos, target_orientation=rot_vector_new, orientation_mode=toolSlot_axis) + return ikResults + diff --git a/default/controllers/generic_inverse_kinematic/spawn_target.py b/default/controllers/generic_inverse_kinematic/spawn_target.py new file mode 100644 index 0000000..b760d03 --- /dev/null +++ b/default/controllers/generic_inverse_kinematic/spawn_target.py @@ -0,0 +1,59 @@ +# 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. + +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/checkered_marble.jpg" + ] + } + roughnessMap ImageTexture { + url [ + "textures/checkered_marble.jpg" + ] + } + metalnessMap ImageTexture { + url [ + "textures/checkered_marble.jpg" + ] + } + emissiveColorMap ImageTexture { + url [ + "textures/checkered_marble.jpg" + ] + } + textureTransform TextureTransform { + scale 2 1 + } + } + geometry Sphere { + radius 0.1 + subdivision 2 + } + } + ] + } + """ + root = supervisor.getRoot() + rootChildren = root.getField('children') + rootChildren.importMFNodeFromString(rootChildren.getCount(), targetSphereString) \ No newline at end of file From e2728545f3c5b1897d5f73421f3c6bdb97ce3cbc Mon Sep 17 00:00:00 2001 From: Simon Steinmann Date: Tue, 18 Aug 2020 19:33:25 +0200 Subject: [PATCH 08/12] cleanup last commit --- .../generic_inverse_kinematic copy.py | 202 ------------------ .../generic_inverse_kinematic.py | 14 -- 2 files changed, 216 deletions(-) delete mode 100644 default/controllers/generic_inverse_kinematic/generic_inverse_kinematic copy.py diff --git a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic copy.py b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic copy.py deleted file mode 100644 index 6e830ae..0000000 --- a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic copy.py +++ /dev/null @@ -1,202 +0,0 @@ -# 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. - -"""Demonstration of inverse kinematics using the "ikpy" Python module.""" - -import sys -import tempfile -from math import sin, pi -import math -import numpy as np -from controller import Supervisor -from get_relative_position import RelativePositions -from scipy.spatial.transform import Rotation as R - -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"') - - -# ---------------------------------------------------------- -# 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() -timeStep = int(supervisor.getBasicTimeStep()) - -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/checkered_marble.jpg" - ] - } - roughnessMap ImageTexture { - url [ - "textures/checkered_marble.jpg" - ] - } - metalnessMap ImageTexture { - url [ - "textures/checkered_marble.jpg" - ] - } - emissiveColorMap ImageTexture { - url [ - "textures/checkered_marble.jpg" - ] - } - textureTransform TextureTransform { - scale 2 1 - } - } - geometry Sphere { - radius 0.1 - subdivision 2 - } - } - ] -} -""" -target = supervisor.getFromDef('TARGET') -try: - target.getPosition() -except: - print('No TARGET defined. Spawning TARGET sphere') - root = supervisor.getRoot() - rootChildren = root.getField('children') - rootChildren.importMFNodeFromString(rootChildren.getCount(), targetSphereString) - - - -# Initialize the RelativePositions class -RelPos = RelativePositions(supervisor) - -# Initialize the arm motors and sensors. -n = supervisor.getNumberOfDevices() -motors = [] -sensors = [] -motor_names = [] -sensor_names = [] -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()) - motor_names.append(device.getName()) - sensor_names.append(sensors[-1].getName()) - sensors[-1].enable(timeStep) - - - - -# Create the arm chain. -filename = None -with tempfile.NamedTemporaryFile(suffix='.urdf', delete=False) as file: - filename = file.name - file.write(supervisor.getUrdf().encode('utf-8')) -armChain = ikpy.chain.Chain.from_urdf_file(filename) - - - - -# make sure only links with a motor are active for IK calculations -active_links = [False] * len(armChain.links) - -for i in range(len(armChain.links)): - link_name = armChain.links[i].name - active_links[i] = link_name in motor_names or link_name in sensor_names - if active_links[i]: - # ikpy includes the bounds as valid, In Webots they have to be less than the limit - new_lower = armChain.links[i].bounds[0] + 0.0000001 - new_upper = armChain.links[i].bounds[1] - 0.0000001 - armChain.links[i].bounds = (new_lower, new_upper) - -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', armChain.links[-1].orientation, degrees=False).as_matrix() -# get the translation vector of the last link -pos = armChain.links[-1].translation_vector -# calculate the final link vector using the dot product -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: - rot_index = final_link_vector.tolist().index(np.sum(final_link_vector)) -except: - 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:', final_link_vector) - -# define for ikpy, which axis we are using -toolSlot_axis = ['X', 'Y', 'Z'][rot_index] - - -# ---------------------------------------------------------- -# Main loop, following the target sphere -# ---------------------------------------------------------- - -target_pos_old = np.zeros((3)) -target_rot_old = [0]*3 -print('Move the yellow and black 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') - - # get the rotation vector from the target_rot rotation matrix, depending on which axis points out of the toolSlot - rot_vector = [target_rot[0,rot_index],target_rot[1,rot_index],target_rot[2,rot_index]] - - # Call "ikpy" to compute the inverse kinematics of the arm. - if any(target_pos != target_pos_old) or rot_vector != target_rot_old: - ikResults = armChain.inverse_kinematics(target_pos, target_orientation=rot_vector, orientation_mode=toolSlot_axis) - if sum(ikResults) == 0: # dealing with singularity - rot_vector_new = np.array(rot_vector) + np.full((1,3), 0.0000001) # tiny change in the orientation vector to avoid singularity - ikResults = armChain.inverse_kinematics(target_pos, target_orientation=rot_vector_new, orientation_mode=toolSlot_axis) - for i in range(len(motors)): - motors[i].setPosition(ikResults[i + 1]) - target_pos_old = target_pos - target_rot_old = rot_vector - diff --git a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py index ee2c5ef..8401e62 100644 --- a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py +++ b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py @@ -14,28 +14,14 @@ """Demonstration of inverse kinematics using the "ikpy" Python module.""" -import sys -import tempfile import numpy as np from controller import Supervisor -from scipy.spatial.transform import Rotation as R # 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 -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"') - - # ---------------------------------------------------------- # CONFIGURATION # ---------------------------------------------------------- From 25c0989503ce1cd374354433f32cebdbcbfcdaf0 Mon Sep 17 00:00:00 2001 From: Simon Steinmann Date: Tue, 18 Aug 2020 20:20:12 +0200 Subject: [PATCH 09/12] copy texture.png to worlds/textures folder --- .../generic_inverse_kinematic.py | 11 +++++---- .../get_relative_position.py | 1 - .../generic_inverse_kinematic/ik_module.py | 12 ++++++---- .../generic_inverse_kinematic/spawn_target.py | 21 ++++++++++++++---- .../textures/target.png | Bin 0 -> 230 bytes 5 files changed, 32 insertions(+), 13 deletions(-) create mode 100644 default/controllers/generic_inverse_kinematic/textures/target.png diff --git a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py index 8401e62..7710526 100644 --- a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py +++ b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py @@ -12,8 +12,7 @@ # See the License for the specific language governing permissions and # limitations under the License. -"""Demonstration of inverse kinematics using the "ikpy" Python module.""" - +import sys import numpy as np from controller import Supervisor @@ -35,9 +34,14 @@ # 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() @@ -46,8 +50,7 @@ spawnTarget(supervisor) - -# Initialize the RelativePositions class +# Initialize the RelativePositions module RelPos = RelativePositions(supervisor) # Initialize the arm motors and sensors. diff --git a/default/controllers/generic_inverse_kinematic/get_relative_position.py b/default/controllers/generic_inverse_kinematic/get_relative_position.py index e9f3cc4..e35bfd7 100644 --- a/default/controllers/generic_inverse_kinematic/get_relative_position.py +++ b/default/controllers/generic_inverse_kinematic/get_relative_position.py @@ -1,6 +1,5 @@ import numpy as np - class RelativePositions(): def __init__(self, Supervisor): self.robot = Supervisor diff --git a/default/controllers/generic_inverse_kinematic/ik_module.py b/default/controllers/generic_inverse_kinematic/ik_module.py index d1dfef1..776258d 100644 --- a/default/controllers/generic_inverse_kinematic/ik_module.py +++ b/default/controllers/generic_inverse_kinematic/ik_module.py @@ -96,16 +96,20 @@ def __init__(self, supervisor): 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. + # 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 sum(ikResults) == 0: # dealing with singularity - rot_vector_new = np.array(rot_vector) + np.full((1,3), 0.0000001) # tiny change in the orientation vector to avoid singularity + + # 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 - diff --git a/default/controllers/generic_inverse_kinematic/spawn_target.py b/default/controllers/generic_inverse_kinematic/spawn_target.py index b760d03..28e0774 100644 --- a/default/controllers/generic_inverse_kinematic/spawn_target.py +++ b/default/controllers/generic_inverse_kinematic/spawn_target.py @@ -11,6 +11,8 @@ # 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): @@ -24,22 +26,22 @@ def __init__(self, supervisor): appearance PBRAppearance { baseColorMap ImageTexture { url [ - "textures/checkered_marble.jpg" + "textures/target.png" ] } roughnessMap ImageTexture { url [ - "textures/checkered_marble.jpg" + "textures/target.png" ] } metalnessMap ImageTexture { url [ - "textures/checkered_marble.jpg" + "textures/target.png" ] } emissiveColorMap ImageTexture { url [ - "textures/checkered_marble.jpg" + "textures/target.png" ] } textureTransform TextureTransform { @@ -54,6 +56,17 @@ def __init__(self, supervisor): ] } """ + # 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) \ No newline at end of file diff --git a/default/controllers/generic_inverse_kinematic/textures/target.png b/default/controllers/generic_inverse_kinematic/textures/target.png new file mode 100644 index 0000000000000000000000000000000000000000..ae126662c24edf49122e9a0cd9415fde0187924f GIT binary patch literal 230 zcmeAS@N?(olHy`uVBq!ia0vp^4j|0I1SD0tpLGH$&H|6fVg?31We{epSZZGe6l5>) z^mS!_%*?~ZBv734@d{8#vcxr_#5q4VH#M(>!MP|ku_QG`p**uBL&4qCHz2%`PaLQy z!PCVtB;(%O8-~0L3Oud{#rR`CuJ0BVUZK(WWL|^X^!=ubckWGp{a0{4J5V(yDA@nz j-~Di1ZcG`F$eyc=%Tid2o2Qo@193fF{an^LB{Ts5w&6K9 literal 0 HcmV?d00001 From 019ece9d14ef91407b2946ac480cbe46dc05cffc Mon Sep 17 00:00:00 2001 From: Simon Steinmann Date: Fri, 28 Aug 2020 14:47:46 +0200 Subject: [PATCH 10/12] Add full rotation and fix bugs --- .../generic_inverse_kinematic.py | 19 +++- .../get_relative_position.py | 8 +- .../generic_inverse_kinematic/ik_module.py | 92 +++++++++++++++---- .../generic_inverse_kinematic/spawn_target.py | 7 +- 4 files changed, 97 insertions(+), 29 deletions(-) diff --git a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py index 7710526..c7ef979 100644 --- a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py +++ b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py @@ -28,6 +28,9 @@ # is only relevant, if the target is constantly changing, as no new IK # solution gets calculated, if the target did not change. IKstepSize = 10 +useRotation = True +last_link_vector = None#[0, 0.12, 0] + # ---------------------------------------------------------- # ---------------------------------------------------------- @@ -39,7 +42,7 @@ timeStep = int(supervisor.getBasicTimeStep()) # Initialize our inverse kinematics module -ik = inverseKinematics(supervisor) +ik = inverseKinematics(supervisor,last_link_vector) # check if our world already has the TARGET node. If not, we spawn it. target = supervisor.getFromDef('TARGET') @@ -62,8 +65,13 @@ #print(device.getName(), ' - NodeType:', device.getNodeType()) if device.getNodeType() == 54: motors.append(device) - sensors.append(device.getPositionSensor()) - sensors[-1].enable(timeStep) + sensor = device.getPositionSensor() + try: + sensor.getName() + sensors.append(sensor) + sensor.enable(timeStep) + except: + print('Rotational Motor: ' + device.getName() + ' has no Position Sensor') target_pos_old = np.zeros((3)) @@ -76,7 +84,10 @@ # 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) + if useRotation: + ikResults = ik.get_ik(target_pos, target_rot) + else: + ikResults = ik.get_ik(target_pos) # set the motor positions to the calculated ik solution for i in range(len(motors)): motors[i].setPosition(ikResults[i + 1]) diff --git a/default/controllers/generic_inverse_kinematic/get_relative_position.py b/default/controllers/generic_inverse_kinematic/get_relative_position.py index e35bfd7..4ee9508 100644 --- a/default/controllers/generic_inverse_kinematic/get_relative_position.py +++ b/default/controllers/generic_inverse_kinematic/get_relative_position.py @@ -4,9 +4,13 @@ class RelativePositions(): def __init__(self, Supervisor): self.robot = Supervisor - def get_pos(self, DEF_target): - base = self.robot.getSelf() + def get_pos(self, DEF_target, DEF_base=None): target = self.robot.getFromDef(DEF_target) + if DEF_base is None: + base = self.robot.getSelf() + else: + base = self.robot.getFromDef(DEF_base) + # 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 diff --git a/default/controllers/generic_inverse_kinematic/ik_module.py b/default/controllers/generic_inverse_kinematic/ik_module.py index 776258d..2c3699d 100644 --- a/default/controllers/generic_inverse_kinematic/ik_module.py +++ b/default/controllers/generic_inverse_kinematic/ik_module.py @@ -32,7 +32,7 @@ 'Please upgrade "ikpy" Python module to version "3.0" or newer with this command: "pip install --upgrade ikpy"') class inverseKinematics(): - def __init__(self, supervisor): + def __init__(self, supervisor, last_link_vector=None): # Initialize the Webots Supervisor. self.supervisor = supervisor self.timeStep = int(self.supervisor.getBasicTimeStep()) @@ -43,41 +43,62 @@ def __init__(self, supervisor): self.sensor_names = [] for i in range(n): device = self.supervisor.getDeviceByIndex(i) + print(device.getName() , ' - NodeType = ' , device.getNodeType()) if device.getNodeType() == 54: sensor = device.getPositionSensor() self.motor_names.append(device.getName()) - self.sensor_names.append(sensor.getName()) + try: + self.sensor_names.append(sensor.getName()) + except: + print('Rotational Motor: ' + device.getName() + ' has no Position Sensor') - # Create the arm chain. + # Create the arm chain. filename = None + # (uncomment next two lines, if you want to save a copy of the generated urdf file) + with open('filename.urdf', 'w') as file: + file.write(supervisor.getUrdf()) 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) + file.write(self.supervisor.getUrdf().encode('utf-8')) + self.armChain = ikpy.chain.Chain.from_urdf_file(filename) #,last_link_vector=last_link_vector) + # 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)): + 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) + new_lower = new_upper = None + if self.armChain.links[i].bounds[0] is not None: + new_lower = self.armChain.links[i].bounds[0] + 0.0000001 + if self.armChain.links[i].bounds[1] is not None: + new_upper = self.armChain.links[i].bounds[1] - 0.0000001 + self.armChain.links[i].bounds = (new_lower, new_upper) + self.last_active = i + if not any(active_links): + print('WARNING: could not identify which links are active. Setting all links to active by default.') + active_links = [True] * len(self.armChain.links) self.armChain.active_links_mask = active_links + print(self.armChain) # ---------------------------------------------------------- # 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() + try: + self.last_link_rot = R.from_euler('xyz', self.armChain.links[-1].orientation, degrees=False).as_matrix() + except: + print('Last link has no orientation. Parsing identity matrix (no rotation).') + self.last_link_rot = np.identity(3) # 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) + self.final_link_vector = np.round(np.dot(self.last_link_rot,pos), 2) + print('---final vector: ',self.final_link_vector) # which column we have to take from the rotation matrix, for our toolSlot axis try: @@ -87,13 +108,37 @@ def __init__(self, supervisor): 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] + self.direction = None + + + def rotateToolSlot(self, target_rot, ikResults): + # calculate the difference in Rotation around the axis between what we requested, and what we computed + fk = self.armChain.forward_kinematics(ikResults) + # rotation of the calculated ik solution + R_fk = fk[:3,:3] + # rotation to get from our ik rotation to our target rotation + R_new = np.dot(target_rot, R_fk) + # convert our rotation matrix into euler angles, so we know how much we have to change the last motor + r = R.from_matrix(R_new) + euler = r.as_euler('xyz', False) + # the first time we call this function we have to figure out in which direction we have to rotate + if self.direction is not None: + ikResults[6] = ikResults[self.last_active ] + self.direction * euler[self.rot_index] + else: + # here we test adding the delta-angle, if the resulting rotation - target rotation is not 0, we flip the direction + ikResults[6] = ikResults[self.last_active ] + 1 * euler[self.rot_index] + fk_check = self.armChain.forward_kinematics(ikResults) + if np.sum(np.abs(np.round(fk_check[:3, :3] - target_rot, 3))) == 0: + self.direction = 1 + else: + self.direction = -1 + return ikResults + def get_ik(self, target_pos, target_rot=None): # check if a target_rot is requested (3x3 numpy array) @@ -105,11 +150,22 @@ def get_ik(self, target_pos, target_rot=None): 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 + transMat = np.identity(4) + transMat[:3,:3] = target_rot + transMat[:3,3] = target_pos 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: + if sum(abs(ikResults)) < 0.001: # 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 + 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=self.toolSlot_axis) + # if the above step was not enough to avoid the singularity, we try to increase the deviation from the requested orientation step by step + n = -5 + while sum(abs(ikResults)) < 0.001: + print('Simple singulaity avoidance was not suffiecient. Trying larger deviation from requested orientation.') + print('Requested rotation vector: ' + str(rot_vector)) + rot_vector_new = np.array(rot_vector) + np.full((1,3), 1 * 10 ** n) + print('New rotation vector: ' + str(rot_vector_new)) + ikResults = self.armChain.inverse_kinematics(target_pos, target_orientation=rot_vector_new, orientation_mode=self.toolSlot_axis) + n += 1 + return self.rotateToolSlot(target_rot, ikResults) diff --git a/default/controllers/generic_inverse_kinematic/spawn_target.py b/default/controllers/generic_inverse_kinematic/spawn_target.py index 28e0774..b9d99dd 100644 --- a/default/controllers/generic_inverse_kinematic/spawn_target.py +++ b/default/controllers/generic_inverse_kinematic/spawn_target.py @@ -56,12 +56,9 @@ def __init__(self, supervisor): ] } """ - # copy the target.png texture into our wold - filePath = supervisor.getWorldPath() - fileNameLength = len(filePath.split('/')[-1]) - worldPath = filePath[:-fileNameLength] + # copy the target.png texture into our wold controllerPath = os.path.dirname(os.path.abspath(__file__)) - worldPath = worldPath + 'textures/' + worldPath = os.path.dirname(supervisor.getWorldPath())+ '/textures/' if not os.path.exists(worldPath): os.makedirs(worldPath) copyfile(controllerPath + '/textures/target.png', worldPath + 'target.png') From c29de7ce2e06ec8dc0c90aceeebb3a65332949ce Mon Sep 17 00:00:00 2001 From: Simon Steinmann Date: Fri, 28 Aug 2020 14:54:38 +0200 Subject: [PATCH 11/12] Configuration documentation updated --- .../generic_inverse_kinematic.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py index c7ef979..13785f3 100644 --- a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py +++ b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py @@ -28,8 +28,14 @@ # is only relevant, if the target is constantly changing, as no new IK # solution gets calculated, if the target did not change. IKstepSize = 10 + +# Whether we want to want to solve for the Target rotation useRotation = True -last_link_vector = None#[0, 0.12, 0] + +# For some robots, the last link might be a motor (the last active_links value is False). +# If this is the case, you can add a translation vector, from the last motor join to the +# Toolslot. The uncommented example is for the UR10e +last_link_vector = None # [0, 0.12, 0] # ---------------------------------------------------------- From 23794586b789dabfb43baec4d018bf0556d7edf4 Mon Sep 17 00:00:00 2001 From: Simon Steinmann Date: Fri, 28 Aug 2020 15:04:19 +0200 Subject: [PATCH 12/12] pep8 formatting --- .../generic_inverse_kinematic.py | 30 +++++++++---------- .../generic_inverse_kinematic/spawn_target.py | 7 +++-- 2 files changed, 18 insertions(+), 19 deletions(-) diff --git a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py index 13785f3..c31fdad 100644 --- a/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py +++ b/default/controllers/generic_inverse_kinematic/generic_inverse_kinematic.py @@ -24,10 +24,10 @@ # ---------------------------------------------------------- # CONFIGURATION # ---------------------------------------------------------- -# how many simulationsteps before calculating the next IK solution. This +# 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 +IKstepSize = 10 # Whether we want to want to solve for the Target rotation useRotation = True @@ -48,55 +48,53 @@ timeStep = int(supervisor.getBasicTimeStep()) # Initialize our inverse kinematics module -ik = inverseKinematics(supervisor,last_link_vector) +ik = inverseKinematics(supervisor, last_link_vector) # check if our world already has the TARGET node. If not, we spawn it. target = supervisor.getFromDef('TARGET') try: target.getPosition() -except: +except Exception as e: print('No TARGET defined. Spawning TARGET sphere') spawnTarget(supervisor) - # Initialize the RelativePositions module RelPos = RelativePositions(supervisor) -# Initialize the arm motors and sensors. +# 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()) + # print(device.getName(), ' - NodeType:', device.getNodeType()) if device.getNodeType() == 54: motors.append(device) sensor = device.getPositionSensor() try: sensor.getName() sensors.append(sensor) - sensor.enable(timeStep) - except: + sensor.enable(timeStep) + except Exception as e: print('Rotational Motor: ' + device.getName() + ' has no Position Sensor') - - + + target_pos_old = np.zeros((3)) -target_rot_old = np.zeros((3,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') + 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. + # Call the ik_module to compute the inverse kinematics of the arm. if useRotation: ikResults = ik.get_ik(target_pos, target_rot) - else: + else: ikResults = ik.get_ik(target_pos) # 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 - diff --git a/default/controllers/generic_inverse_kinematic/spawn_target.py b/default/controllers/generic_inverse_kinematic/spawn_target.py index b9d99dd..b474699 100644 --- a/default/controllers/generic_inverse_kinematic/spawn_target.py +++ b/default/controllers/generic_inverse_kinematic/spawn_target.py @@ -14,6 +14,7 @@ import os from shutil import copyfile + class spawnTarget(): def __init__(self, supervisor): targetSphereString = """ @@ -56,9 +57,9 @@ def __init__(self, supervisor): ] } """ - # copy the target.png texture into our wold + # copy the target.png texture into our wold controllerPath = os.path.dirname(os.path.abspath(__file__)) - worldPath = os.path.dirname(supervisor.getWorldPath())+ '/textures/' + worldPath = os.path.dirname(supervisor.getWorldPath()) + '/textures/' if not os.path.exists(worldPath): os.makedirs(worldPath) copyfile(controllerPath + '/textures/target.png', worldPath + 'target.png') @@ -66,4 +67,4 @@ def __init__(self, supervisor): # spawn the TARGET node root = supervisor.getRoot() rootChildren = root.getField('children') - rootChildren.importMFNodeFromString(rootChildren.getCount(), targetSphereString) \ No newline at end of file + rootChildren.importMFNodeFromString(rootChildren.getCount(), targetSphereString)