Merge pull request #109 from Phylliade/phylliade/prismatic_joint

Release v3.2
Phylliade · Jul 10, 2021 · 3e879d1 · 3e879d1
2 parents cd6775a + 1aefdf3
commit 3e879d1
Show file tree

Hide file tree

Showing 25 changed files with 585 additions and 106 deletions.
diff --git a/README.md b/README.md
@@ -26,6 +26,7 @@ With IKPy, you can:
 * Compute the Inverse Kinematics in **position, [orientation](./tutorials/Orientation.ipynb)**, or both
 * Define your kinematic chain using **arbitrary representations**: DH (Denavit–Hartenberg), URDF, custom...
 * Automaticly import a kinematic chain from a **URDF file**.
+* Support for arbitrary joint types: `revolute`, `prismatic` and more to come in the future 
 * Use pre-configured robots, such as [**baxter**](./tutorials/Baxter%20kinematics.ipynb) or the **poppy-torso**
 * IKPy is **precise** (up to 7 digits): the only limitation being your underlying model's precision, and **fast**: from 7 ms to 50 ms (depending on your precision) for a complete IK computation.
 * **Plot** your kinematic chain: no need to use a real robot (or a simulator) to test your algorithms!

diff --git a/contrib/transformations.py b/contrib/transformations.py
@@ -55,13 +55,13 @@ def pose_to_list(pose):
         return [[
             pose.pose.position.x, pose.pose.position.y, pose.pose.position.z
         ], [
-            pose.pose.orientation.x, pose.pose.orientation.y,
-            pose.pose.orientation.z, pose.pose.orientation.w
+            pose.pose.origin_orientation.x, pose.pose.origin_orientation.y,
+            pose.pose.origin_orientation.z, pose.pose.origin_orientation.w
         ]]
     elif type(pose) == geometry_msgs.msg.Pose:
         return [[pose.position.x, pose.position.y, pose.position.z], [
-            pose.orientation.x, pose.orientation.y, pose.orientation.z,
-            pose.orientation.w
+            pose.origin_orientation.x, pose.origin_orientation.y, pose.origin_orientation.z,
+            pose.origin_orientation.w
         ]]
     else:
         raise Exception("pose_to_list: parameter of type %s unexpected",
@@ -92,10 +92,10 @@ def list_to_pose(poselist, frame_id="", stamp=rospy.Time(0)):
     p.pose.position.x = poselist[0][0]
     p.pose.position.y = poselist[0][1]
     p.pose.position.z = poselist[0][2]
-    p.pose.orientation.x = poselist[1][0]
-    p.pose.orientation.y = poselist[1][1]
-    p.pose.orientation.z = poselist[1][2]
-    p.pose.orientation.w = poselist[1][3]
+    p.pose.origin_orientation.x = poselist[1][0]
+    p.pose.origin_orientation.y = poselist[1][1]
+    p.pose.origin_orientation.z = poselist[1][2]
+    p.pose.origin_orientation.w = poselist[1][3]
     return p
 
 

diff --git a/resources/baxter/baxter.pdf b/resources/baxter/baxter.pdf
diff --git a/resources/baxter/baxter_head.json b/resources/baxter/baxter_head.json
@@ -12,12 +12,12 @@
   ],
   "urdf_file": "baxter.urdf",
   "active_links_mask": [
+    false,
+    false,
     true,
-    true,
-    true,
-    true,
-    true,
-    true
+    false,
+    false,
+    false
   ],
   "last_link_vector": [
     0,

diff --git a/resources/baxter/baxter_left_arm.json b/resources/baxter/baxter_left_arm.json
@@ -38,9 +38,9 @@
     true,
     true,
     true,
-    true,
-    true,
-    true,
+    false,
+    false,
+    false,
     false
   ],
   "last_link_vector": [

diff --git a/resources/baxter/baxter_pedestal.json b/resources/baxter/baxter_pedestal.json
@@ -8,10 +8,10 @@
   ],
   "urdf_file": "baxter.urdf",
   "active_links_mask": [
-    true,
-    true,
-    true,
-    true
+    false,
+    false,
+    false,
+    false
   ],
   "last_link_vector": [
     0,

diff --git a/resources/baxter/baxter_right_arm.json b/resources/baxter/baxter_right_arm.json
@@ -38,9 +38,9 @@
     true,
     true,
     true,
-    true,
-    true,
-    true,
+    false,
+    false,
+    false,
     false
   ],
   "last_link_vector": [

diff --git a/resources/baxter/generate_chains.py b/resources/baxter/generate_chains.py
@@ -1,14 +1,9 @@
-from graphviz import Digraph
-import numpy as np
-import json
-
 # IKPy imports
 from ikpy import chain
-from ikpy.utils import plot
 from ikpy.urdf.utils import get_urdf_tree
 
 # Generate the pdf
-dot, urdf_tree = get_urdf_tree("./baxter.urdf", out_image_path="./baxter")
+dot, urdf_tree = get_urdf_tree("./baxter.urdf", out_image_path="./baxter", root_element="base")
 
 
 ########################## Left arm ##########################
@@ -49,7 +44,7 @@
     "./baxter.urdf",
     base_elements=baxter_left_arm_elements,
     last_link_vector=[0, 0.18, 0],
-    active_links_mask=3 * [False] + 11 * [True],
+    active_links_mask=3 * [False] + 7 * [True] + 4 * [False],
     symbolic=False,
     name="baxter_left_arm")
 baxter_left_arm_chain.to_json_file(force=True)
@@ -62,7 +57,7 @@
     "./baxter.urdf",
     base_elements=baxter_right_arm_elements,
     last_link_vector=[0, 0.18, 0],
-    active_links_mask=3 * [False] + 11 * [True],
+    active_links_mask=3 * [False] + 7 * [True] + 4 * [False],
     symbolic=False,
     name="baxter_right_arm")
 baxter_right_arm_chain.to_json_file(force=True)
@@ -84,7 +79,9 @@
     base_elements=baxter_head_elements,
     last_link_vector=[0, 0.18, 0],
     symbolic=False,
-    name="baxter_head")
+    name="baxter_head",
+    active_links_mask=[False, False, True, False, False, False]
+)
 baxter_head_chain.to_json_file(force=True)
 
 ########################## Pedestal ###############################
@@ -100,5 +97,6 @@
     base_elements=baxter_pedestal_elements,
     last_link_vector=[0, 0.18, 0],
     symbolic=False,
-    name="baxter_pedestal")
+    name="baxter_pedestal",
+    active_links_mask=[False, False, False, True])
 baxter_pedestal_chain.to_json_file(force=True)
diff --git a/resources/prismatic/generate_chains.py b/resources/prismatic/generate_chains.py
@@ -0,0 +1,47 @@
+# IKPy imports
+from ikpy import chain
+
+
+########################## Simple prismatic robot ###############################
+
+prismatic_robot_chain = chain.Chain.from_urdf_file(
+    "./prismatic_robot.URDF",
+    base_elements=[
+            "baseLink", "joint_baseLink_childA", "childA"
+    ],
+    last_link_vector=[0, 1, 0],
+    active_links_mask=[False, True, False],
+    name="prismatic_robot",
+)
+
+prismatic_robot_chain.to_json_file(force=True)
+print("Saved chain: ", prismatic_robot_chain)
+
+########################## Prismatic mixed robot ###############################
+
+prismatic_mixed_robot_elements = [
+    "base_link",
+    "base_link_base_slider_joint",
+    "base_slider",
+    "linear_actuator",
+    "robot_base",
+    "arm2",
+    "link_2",
+    "arm3",
+    "link_3",
+    "arm4",
+    "link_4",
+    "arm5",
+    "link_5",
+]
+
+prismatic_mixed_robot_chain = chain.Chain.from_urdf_file(
+    "./prismatic_mixed_robot.URDF",
+    base_elements=prismatic_mixed_robot_elements,
+    last_link_vector=[0, 0.18, 0],
+    symbolic=False,
+    name="prismatic_mixed_robot",
+    active_links_mask=2 * [False] + [True] * 5 + [False])
+
+prismatic_mixed_robot_chain.to_json_file(force=True)
+print("Saved chain: ", prismatic_mixed_robot_chain)
diff --git a/resources/prismatic/prismatic_mixed_robot.URDF b/resources/prismatic/prismatic_mixed_robot.URDF
@@ -0,0 +1,124 @@
+<?xml version="1.0"?>
+<robot name="robot" xmlns:xacro="http://ros.org/wiki/xacro">
+  <link name="base_link">
+  </link>
+  <link name="base_slider">
+  </link>
+  <joint name="base_link_base_slider_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="base_slider"/>
+    <origin xyz="0 0 0" rpy="0 -1.5707953 0"/>
+  </joint>
+  <joint name="linear_actuator" type="prismatic">
+    <parent link="base_slider"/>
+    <child link="robot_base"/>
+    <axis xyz="-1 -2.368514e-06 4.1005696e-06"/>
+    <origin xyz="0 0 0" rpy="-0.5237906 1.5707916 1.0470034"/>
+    <limit effort="10" lower="0" upper="0.4" velocity="10"/>
+  </joint>
+  <link name="robot_base">
+    <visual>
+      <origin xyz="0.032 0.023 0.07" rpy="1.5707963 0 0"/>
+      <geometry>
+        <cylinder radius="0.036" length="0.046"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0.032 0.023 0.07" rpy="1.5707963 0 0"/>
+      <geometry>
+        <cylinder radius="0.036" length="0.046"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm2" type="revolute">
+    <parent link="robot_base"/>
+    <child link="link_2"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="9.5" lower="-1.13446" upper="1.5708" velocity="1.5708"/>
+    <origin xyz="0.033 0 0.07" rpy="0 0 0"/>
+  </joint>
+  <link name="link_2">
+    <visual>
+      <origin xyz="0 -0.03 0.078" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.07 0.06 0.21"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.03 0.078" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.07 0.06 0.21"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm3" type="revolute">
+    <parent link="link_2"/>
+    <child link="link_3"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="6" lower="-2.63545" upper="2.54818" velocity="1.5708"/>
+    <origin xyz="0 0 0.155" rpy="0 0 0"/>
+  </joint>
+  <link name="link_3">
+    <visual>
+      <origin xyz="0 0.0225 0.065" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.06 0.045 0.19"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0.0225 0.065" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.06 0.045 0.19"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm4" type="revolute">
+    <parent link="link_3"/>
+    <child link="link_4"/>
+    <axis xyz="0 -1 0"/>
+    <limit effort="2" lower="-1.78024" upper="1.78024" velocity="1.5708"/>
+    <origin xyz="0 0 0.135" rpy="0 0 0"/>
+  </joint>
+  <link name="link_4">
+    <visual>
+      <origin xyz="0 0 0.056" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.095 0.05"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0.056" rpy="0 0 0"/>
+      <geometry>
+        <box size="0.05 0.095 0.05"/>
+      </geometry>
+    </collision>
+    <visual>
+      <origin xyz="0 -0.024 0.003" rpy="1.5707963 0 0"/>
+      <geometry>
+        <cylinder radius="0.027" length="0.047"/>
+      </geometry>
+    </visual>
+    <collision>
+      <origin xyz="0 -0.024 0.003" rpy="1.5707963 0 0"/>
+      <geometry>
+        <cylinder radius="0.027" length="0.047"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="arm5" type="revolute">
+    <parent link="link_4"/>
+    <child link="link_5"/>
+    <axis xyz="0 0 1"/>
+    <limit effort="1" lower="-2.92343" upper="2.92343" velocity="1.5708"/>
+    <origin xyz="0 0 0.081" rpy="0 0 0"/>
+  </joint>
+  <link name="link_5">
+  </link>
+  <link name="platform">
+  </link>
+  <joint name="base_link_platform_joint" type="fixed">
+    <parent link="base_link"/>
+    <child link="platform"/>
+    <origin xyz="-0.24 -0.01 0" rpy="3.1415927 -1.5707927 3.1415927"/>
+  </joint>
+</robot>
diff --git a/resources/prismatic/prismatic_mixed_robot.json b/resources/prismatic/prismatic_mixed_robot.json
@@ -0,0 +1,35 @@
+{
+  "elements": [
+    "base_link",
+    "base_link_base_slider_joint",
+    "base_slider",
+    "linear_actuator",
+    "robot_base",
+    "arm2",
+    "link_2",
+    "arm3",
+    "link_3",
+    "arm4",
+    "link_4",
+    "arm5",
+    "link_5"
+  ],
+  "urdf_file": "prismatic_mixed_robot.URDF",
+  "active_links_mask": [
+    false,
+    false,
+    true,
+    true,
+    true,
+    true,
+    true,
+    false
+  ],
+  "last_link_vector": [
+    0,
+    0.18,
+    0
+  ],
+  "name": "prismatic_mixed_robot",
+  "version": "v1"
+}