Skip to content

Commit 3e879d1

Browse files
PhylliadePhylliade
authored
Merge pull request #109 from Phylliade/phylliade/prismatic_joint
Release v3.2
2 parents cd6775a + 1aefdf3 commit 3e879d1

25 files changed

+585
-106
lines changed

‎README.md

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ With IKPy, you can:
2626
* Compute the Inverse Kinematics in **position, [orientation](./tutorials/Orientation.ipynb)**, or both
2727
* Define your kinematic chain using **arbitrary representations**: DH (Denavit–Hartenberg), URDF, custom...
2828
* Automaticly import a kinematic chain from a **URDF file**.
29+
* Support for arbitrary joint types: `revolute`, `prismatic` and more to come in the future
2930
* Use pre-configured robots, such as [**baxter**](./tutorials/Baxter%20kinematics.ipynb) or the **poppy-torso**
3031
* 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.
3132
* **Plot** your kinematic chain: no need to use a real robot (or a simulator) to test your algorithms!

‎contrib/transformations.py

Lines changed: 8 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -55,13 +55,13 @@ def pose_to_list(pose):
5555
return [[
5656
pose.pose.position.x, pose.pose.position.y, pose.pose.position.z
5757
], [
58-
pose.pose.orientation.x, pose.pose.orientation.y,
59-
pose.pose.orientation.z, pose.pose.orientation.w
58+
pose.pose.origin_orientation.x, pose.pose.origin_orientation.y,
59+
pose.pose.origin_orientation.z, pose.pose.origin_orientation.w
6060
]]
6161
elif type(pose) == geometry_msgs.msg.Pose:
6262
return [[pose.position.x, pose.position.y, pose.position.z], [
63-
pose.orientation.x, pose.orientation.y, pose.orientation.z,
64-
pose.orientation.w
63+
pose.origin_orientation.x, pose.origin_orientation.y, pose.origin_orientation.z,
64+
pose.origin_orientation.w
6565
]]
6666
else:
6767
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)):
9292
p.pose.position.x = poselist[0][0]
9393
p.pose.position.y = poselist[0][1]
9494
p.pose.position.z = poselist[0][2]
95-
p.pose.orientation.x = poselist[1][0]
96-
p.pose.orientation.y = poselist[1][1]
97-
p.pose.orientation.z = poselist[1][2]
98-
p.pose.orientation.w = poselist[1][3]
95+
p.pose.origin_orientation.x = poselist[1][0]
96+
p.pose.origin_orientation.y = poselist[1][1]
97+
p.pose.origin_orientation.z = poselist[1][2]
98+
p.pose.origin_orientation.w = poselist[1][3]
9999
return p
100100

101101

‎resources/baxter/baxter.pdf

-3.1 KB
Binary file not shown.

‎resources/baxter/baxter_head.json

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -12,12 +12,12 @@
1212
],
1313
"urdf_file": "baxter.urdf",
1414
"active_links_mask": [
15+
false,
16+
false,
1517
true,
16-
true,
17-
true,
18-
true,
19-
true,
20-
true
18+
false,
19+
false,
20+
false
2121
],
2222
"last_link_vector": [
2323
0,

‎resources/baxter/baxter_left_arm.json

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -38,9 +38,9 @@
3838
true,
3939
true,
4040
true,
41-
true,
42-
true,
43-
true,
41+
false,
42+
false,
43+
false,
4444
false
4545
],
4646
"last_link_vector": [

‎resources/baxter/baxter_pedestal.json

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -8,10 +8,10 @@
88
],
99
"urdf_file": "baxter.urdf",
1010
"active_links_mask": [
11-
true,
12-
true,
13-
true,
14-
true
11+
false,
12+
false,
13+
false,
14+
false
1515
],
1616
"last_link_vector": [
1717
0,

‎resources/baxter/baxter_right_arm.json

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -38,9 +38,9 @@
3838
true,
3939
true,
4040
true,
41-
true,
42-
true,
43-
true,
41+
false,
42+
false,
43+
false,
4444
false
4545
],
4646
"last_link_vector": [

‎resources/baxter/generate_chains.py

Lines changed: 8 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,9 @@
1-
from graphviz import Digraph
2-
import numpy as np
3-
import json
4-
51
# IKPy imports
62
from ikpy import chain
7-
from ikpy.utils import plot
83
from ikpy.urdf.utils import get_urdf_tree
94

105
# Generate the pdf
11-
dot, urdf_tree = get_urdf_tree("./baxter.urdf", out_image_path="./baxter")
6+
dot, urdf_tree = get_urdf_tree("./baxter.urdf", out_image_path="./baxter", root_element="base")
127

138

149
########################## Left arm ##########################
@@ -49,7 +44,7 @@
4944
"./baxter.urdf",
5045
base_elements=baxter_left_arm_elements,
5146
last_link_vector=[0, 0.18, 0],
52-
active_links_mask=3 * [False] + 11 * [True],
47+
active_links_mask=3 * [False] + 7 * [True] + 4 * [False],
5348
symbolic=False,
5449
name="baxter_left_arm")
5550
baxter_left_arm_chain.to_json_file(force=True)
@@ -62,7 +57,7 @@
6257
"./baxter.urdf",
6358
base_elements=baxter_right_arm_elements,
6459
last_link_vector=[0, 0.18, 0],
65-
active_links_mask=3 * [False] + 11 * [True],
60+
active_links_mask=3 * [False] + 7 * [True] + 4 * [False],
6661
symbolic=False,
6762
name="baxter_right_arm")
6863
baxter_right_arm_chain.to_json_file(force=True)
@@ -84,7 +79,9 @@
8479
base_elements=baxter_head_elements,
8580
last_link_vector=[0, 0.18, 0],
8681
symbolic=False,
87-
name="baxter_head")
82+
name="baxter_head",
83+
active_links_mask=[False, False, True, False, False, False]
84+
)
8885
baxter_head_chain.to_json_file(force=True)
8986

9087
########################## Pedestal ###############################
@@ -100,5 +97,6 @@
10097
base_elements=baxter_pedestal_elements,
10198
last_link_vector=[0, 0.18, 0],
10299
symbolic=False,
103-
name="baxter_pedestal")
100+
name="baxter_pedestal",
101+
active_links_mask=[False, False, False, True])
104102
baxter_pedestal_chain.to_json_file(force=True)

‎resources/prismatic/generate_chains.py

Lines changed: 47 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,47 @@
1+
# IKPy imports
2+
from ikpy import chain
3+
4+
5+
########################## Simple prismatic robot ###############################
6+
7+
prismatic_robot_chain = chain.Chain.from_urdf_file(
8+
"./prismatic_robot.URDF",
9+
base_elements=[
10+
"baseLink", "joint_baseLink_childA", "childA"
11+
],
12+
last_link_vector=[0, 1, 0],
13+
active_links_mask=[False, True, False],
14+
name="prismatic_robot",
15+
)
16+
17+
prismatic_robot_chain.to_json_file(force=True)
18+
print("Saved chain: ", prismatic_robot_chain)
19+
20+
########################## Prismatic mixed robot ###############################
21+
22+
prismatic_mixed_robot_elements = [
23+
"base_link",
24+
"base_link_base_slider_joint",
25+
"base_slider",
26+
"linear_actuator",
27+
"robot_base",
28+
"arm2",
29+
"link_2",
30+
"arm3",
31+
"link_3",
32+
"arm4",
33+
"link_4",
34+
"arm5",
35+
"link_5",
36+
]
37+
38+
prismatic_mixed_robot_chain = chain.Chain.from_urdf_file(
39+
"./prismatic_mixed_robot.URDF",
40+
base_elements=prismatic_mixed_robot_elements,
41+
last_link_vector=[0, 0.18, 0],
42+
symbolic=False,
43+
name="prismatic_mixed_robot",
44+
active_links_mask=2 * [False] + [True] * 5 + [False])
45+
46+
prismatic_mixed_robot_chain.to_json_file(force=True)
47+
print("Saved chain: ", prismatic_mixed_robot_chain)

‎resources/prismatic/prismatic_mixed_robot.URDF

Lines changed: 124 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,124 @@
1+
<?xml version="1.0"?>
2+
<robot name="robot" xmlns:xacro="http://ros.org/wiki/xacro">
3+
<link name="base_link">
4+
</link>
5+
<link name="base_slider">
6+
</link>
7+
<joint name="base_link_base_slider_joint" type="fixed">
8+
<parent link="base_link"/>
9+
<child link="base_slider"/>
10+
<origin xyz="0 0 0" rpy="0 -1.5707953 0"/>
11+
</joint>
12+
<joint name="linear_actuator" type="prismatic">
13+
<parent link="base_slider"/>
14+
<child link="robot_base"/>
15+
<axis xyz="-1 -2.368514e-06 4.1005696e-06"/>
16+
<origin xyz="0 0 0" rpy="-0.5237906 1.5707916 1.0470034"/>
17+
<limit effort="10" lower="0" upper="0.4" velocity="10"/>
18+
</joint>
19+
<link name="robot_base">
20+
<visual>
21+
<origin xyz="0.032 0.023 0.07" rpy="1.5707963 0 0"/>
22+
<geometry>
23+
<cylinder radius="0.036" length="0.046"/>
24+
</geometry>
25+
</visual>
26+
<collision>
27+
<origin xyz="0.032 0.023 0.07" rpy="1.5707963 0 0"/>
28+
<geometry>
29+
<cylinder radius="0.036" length="0.046"/>
30+
</geometry>
31+
</collision>
32+
</link>
33+
<joint name="arm2" type="revolute">
34+
<parent link="robot_base"/>
35+
<child link="link_2"/>
36+
<axis xyz="0 -1 0"/>
37+
<limit effort="9.5" lower="-1.13446" upper="1.5708" velocity="1.5708"/>
38+
<origin xyz="0.033 0 0.07" rpy="0 0 0"/>
39+
</joint>
40+
<link name="link_2">
41+
<visual>
42+
<origin xyz="0 -0.03 0.078" rpy="0 0 0"/>
43+
<geometry>
44+
<box size="0.07 0.06 0.21"/>
45+
</geometry>
46+
</visual>
47+
<collision>
48+
<origin xyz="0 -0.03 0.078" rpy="0 0 0"/>
49+
<geometry>
50+
<box size="0.07 0.06 0.21"/>
51+
</geometry>
52+
</collision>
53+
</link>
54+
<joint name="arm3" type="revolute">
55+
<parent link="link_2"/>
56+
<child link="link_3"/>
57+
<axis xyz="0 -1 0"/>
58+
<limit effort="6" lower="-2.63545" upper="2.54818" velocity="1.5708"/>
59+
<origin xyz="0 0 0.155" rpy="0 0 0"/>
60+
</joint>
61+
<link name="link_3">
62+
<visual>
63+
<origin xyz="0 0.0225 0.065" rpy="0 0 0"/>
64+
<geometry>
65+
<box size="0.06 0.045 0.19"/>
66+
</geometry>
67+
</visual>
68+
<collision>
69+
<origin xyz="0 0.0225 0.065" rpy="0 0 0"/>
70+
<geometry>
71+
<box size="0.06 0.045 0.19"/>
72+
</geometry>
73+
</collision>
74+
</link>
75+
<joint name="arm4" type="revolute">
76+
<parent link="link_3"/>
77+
<child link="link_4"/>
78+
<axis xyz="0 -1 0"/>
79+
<limit effort="2" lower="-1.78024" upper="1.78024" velocity="1.5708"/>
80+
<origin xyz="0 0 0.135" rpy="0 0 0"/>
81+
</joint>
82+
<link name="link_4">
83+
<visual>
84+
<origin xyz="0 0 0.056" rpy="0 0 0"/>
85+
<geometry>
86+
<box size="0.05 0.095 0.05"/>
87+
</geometry>
88+
</visual>
89+
<collision>
90+
<origin xyz="0 0 0.056" rpy="0 0 0"/>
91+
<geometry>
92+
<box size="0.05 0.095 0.05"/>
93+
</geometry>
94+
</collision>
95+
<visual>
96+
<origin xyz="0 -0.024 0.003" rpy="1.5707963 0 0"/>
97+
<geometry>
98+
<cylinder radius="0.027" length="0.047"/>
99+
</geometry>
100+
</visual>
101+
<collision>
102+
<origin xyz="0 -0.024 0.003" rpy="1.5707963 0 0"/>
103+
<geometry>
104+
<cylinder radius="0.027" length="0.047"/>
105+
</geometry>
106+
</collision>
107+
</link>
108+
<joint name="arm5" type="revolute">
109+
<parent link="link_4"/>
110+
<child link="link_5"/>
111+
<axis xyz="0 0 1"/>
112+
<limit effort="1" lower="-2.92343" upper="2.92343" velocity="1.5708"/>
113+
<origin xyz="0 0 0.081" rpy="0 0 0"/>
114+
</joint>
115+
<link name="link_5">
116+
</link>
117+
<link name="platform">
118+
</link>
119+
<joint name="base_link_platform_joint" type="fixed">
120+
<parent link="base_link"/>
121+
<child link="platform"/>
122+
<origin xyz="-0.24 -0.01 0" rpy="3.1415927 -1.5707927 3.1415927"/>
123+
</joint>
124+
</robot>

‎resources/prismatic/prismatic_mixed_robot.json

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,35 @@
1+
{
2+
"elements": [
3+
"base_link",
4+
"base_link_base_slider_joint",
5+
"base_slider",
6+
"linear_actuator",
7+
"robot_base",
8+
"arm2",
9+
"link_2",
10+
"arm3",
11+
"link_3",
12+
"arm4",
13+
"link_4",
14+
"arm5",
15+
"link_5"
16+
],
17+
"urdf_file": "prismatic_mixed_robot.URDF",
18+
"active_links_mask": [
19+
false,
20+
false,
21+
true,
22+
true,
23+
true,
24+
true,
25+
true,
26+
false
27+
],
28+
"last_link_vector": [
29+
0,
30+
0.18,
31+
0
32+
],
33+
"name": "prismatic_mixed_robot",
34+
"version": "v1"
35+
}

0 commit comments

Comments
 (0)