Skip to content

Commit 26df352

Browse files
gonzalocasasgonzalocasas
authored
Merge pull request #86 from compas-dev/fix/passive-joints
Fix passive joint cases and multiple active groups
2 parents bf13a0c + 5f7ac02 commit 26df352

File tree

12 files changed

+15407
-90
lines changed

12 files changed

+15407
-90
lines changed

‎CHANGELOG.rst‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@ Unreleased
1717
**Changed**
1818

1919
* Updated to COMPAS 0.10
20+
* Add better support for passive joints on IK, Cartesian and Kinematic planning
2021

2122
**Removed**
2223

‎docs/examples/03_backends_ros/05_full_examples.rst‎

Lines changed: 60 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,60 @@
1+
*******************************************************************************
2+
Full examples
3+
*******************************************************************************
4+
5+
.. note::
6+
7+
The following examples use the `ROS <http://www.ros.org/>`_ backend
8+
and the MoveI! planner with the Franka Emika Panda robots.
9+
Before running them, please make sure you have the :ref:`ROS backend <ros_backend>`
10+
correctly configured and the :ref:`Panda Demo <ros_bundles_list>` started.
11+
12+
The following lists the 4 main features of the integrated motion planning framework:
13+
forward and inverse kinematics, cartesian and kinematic planning including loading
14+
the entire robot model over ROS, and a complete Grasshopper canvas as a playground
15+
for these features.
16+
17+
Forward Kinematics
18+
=====================
19+
20+
.. literalinclude :: files/02_forward_kinematics_ros_loader.py
21+
:language: python
22+
23+
Inverse Kinematics
24+
=====================
25+
26+
.. literalinclude :: files/02_inverse_kinematics_ros_loader.py
27+
:language: python
28+
29+
Plan cartesian motion
30+
=====================
31+
32+
.. literalinclude :: files/04_plan_cartesian_motion_ros_loader.py
33+
:language: python
34+
35+
Plan motion
36+
===========
37+
38+
.. literalinclude :: files/04_plan_motion_ros_loader.py
39+
:language: python
40+
41+
Grasshopper playground
42+
======================
43+
44+
The following Grasshopper canvas demonstrate all these features combined in
45+
a single document, as a reference of how these features could be integrated:
46+
47+
48+
.. raw:: html
49+
50+
<div class="card bg-light">
51+
<div class="card-body">
52+
<div class="card-title">Download</div>
53+
54+
* :download:`Robot playground (Grasshopper) (.GHX) <files/robot-playground.ghx>`
55+
56+
.. raw:: html
57+
58+
</div>
59+
</div>
60+

‎docs/examples/03_backends_ros/files/02_forward_kinematics_ros_loader.py‎

Lines changed: 28 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,28 @@
1+
from compas.robots import RobotModel
2+
3+
from compas_fab.backends import RosClient
4+
from compas_fab.backends import RosFileServerLoader
5+
from compas_fab.robots import Configuration
6+
from compas_fab.robots import Robot
7+
from compas_fab.robots import RobotSemantics
8+
9+
with RosClient() as client:
10+
loader = RosFileServerLoader(client)
11+
12+
urdf = loader.load_urdf()
13+
srdf = loader.load_srdf()
14+
15+
model = RobotModel.from_urdf_string(urdf)
16+
semantics = RobotSemantics.from_srdf_string(srdf, model)
17+
18+
robot = Robot(model, semantics=semantics, client=client)
19+
20+
configuration = Configuration.from_prismatic_and_revolute_values([0.], [-2.238, -1.153, -2.174, 0.185, 0.667, 0.])
21+
22+
frame_RCF = robot.forward_kinematics(configuration)
23+
frame_WCF = robot.to_world_coords(frame_RCF)
24+
25+
print("Frame in the robot's coordinate system")
26+
print(frame_RCF)
27+
print("Frame in the world coordinate system")
28+
print(frame_WCF)

‎docs/examples/03_backends_ros/files/02_inverse_kinematics_ros_loader.py‎

Lines changed: 25 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
from compas.geometry import Frame
2+
from compas.robots import RobotModel
3+
4+
from compas_fab.backends import RosClient
5+
from compas_fab.backends import RosFileServerLoader
6+
from compas_fab.robots import Robot
7+
from compas_fab.robots import RobotSemantics
8+
9+
with RosClient() as client:
10+
loader = RosFileServerLoader(client)
11+
12+
urdf = loader.load_urdf()
13+
srdf = loader.load_srdf()
14+
15+
model = RobotModel.from_urdf_string(urdf)
16+
semantics = RobotSemantics.from_srdf_string(srdf, model)
17+
18+
robot = Robot(model, semantics=semantics, client=client)
19+
20+
frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
21+
start_configuration = robot.init_configuration()
22+
23+
configuration = robot.inverse_kinematics(frame_WCF, start_configuration)
24+
25+
print("Found configuration", configuration)

‎docs/examples/03_backends_ros/files/04_plan_cartesian_motion_ros_loader.py‎

Lines changed: 36 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,36 @@
1+
from compas.geometry import Frame
2+
from compas.robots import RobotModel
3+
4+
from compas_fab.backends import RosClient
5+
from compas_fab.backends import RosFileServerLoader
6+
from compas_fab.robots import Configuration
7+
from compas_fab.robots import Robot
8+
from compas_fab.robots import RobotSemantics
9+
10+
with RosClient() as client:
11+
loader = RosFileServerLoader(client)
12+
13+
urdf = loader.load_urdf()
14+
srdf = loader.load_srdf()
15+
16+
model = RobotModel.from_urdf_string(urdf)
17+
semantics = RobotSemantics.from_srdf_string(srdf, model)
18+
19+
robot = Robot(model, semantics=semantics, client=client)
20+
group = robot.main_group_name
21+
22+
frames = []
23+
frames.append(Frame((0.2925, 0.3949, 0.5066), (0, 1, 0), (0, 0, 1)))
24+
frames.append(Frame((0.5103, 0.2827, 0.4074), (0, 1, 0), (0, 0, 1)))
25+
26+
start_configuration = Configuration.from_revolute_values((0.667, -0.298, 0.336, -2.333, -1.787, 2.123, 0.571))
27+
28+
trajectory = robot.plan_cartesian_motion(frames,
29+
start_configuration,
30+
max_step=0.01,
31+
avoid_collisions=True,
32+
group=group)
33+
34+
print("Computed cartesian path with %d configurations, " % len(trajectory.points))
35+
print("following %d%% of requested trajectory." % (trajectory.fraction * 100))
36+
print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)

‎docs/examples/03_backends_ros/files/04_plan_motion.py‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,3 +27,16 @@
2727

2828
print("Computed kinematic path with %d configurations." % len(trajectory.points))
2929
print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)
30+
# print('Executing')
31+
32+
# robot.client.follow_joint_trajectory(
33+
# trajectory, callback=done_handler, errback=errhandler, feedback_callback=handler)
34+
35+
# wait = robot.client.execute_joint_trajectory(trajectory)
36+
37+
# print('Waiting...')
38+
# r = wait.result()
39+
# print('Done waiting...')
40+
41+
# import time
42+
# time.sleep(1)

‎docs/examples/03_backends_ros/files/04_plan_motion_ros_loader.py‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
import math
2+
from compas.geometry import Frame
3+
from compas.robots import RobotModel
4+
5+
from compas_fab.backends import RosClient
6+
from compas_fab.backends import RosFileServerLoader
7+
from compas_fab.robots import Configuration
8+
from compas_fab.robots import Robot
9+
from compas_fab.robots import RobotSemantics
10+
11+
with RosClient() as client:
12+
loader = RosFileServerLoader(client)
13+
14+
urdf = loader.load_urdf()
15+
srdf = loader.load_srdf()
16+
17+
model = RobotModel.from_urdf_string(urdf)
18+
semantics = RobotSemantics.from_srdf_string(srdf, model)
19+
20+
robot = Robot(model, semantics=semantics, client=client)
21+
group = robot.main_group_name
22+
23+
frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
24+
tolerance_position = 0.001
25+
tolerance_axes = [math.radians(1)] * 3
26+
27+
start_configuration = Configuration.from_revolute_values(
28+
(0.667, -0.298, 0.336, -2.333, -1.787, 2.123, 0.571))
29+
30+
# create goal constraints from frame
31+
goal_constraints = robot.constraints_from_frame(frame,
32+
tolerance_position,
33+
tolerance_axes,
34+
group)
35+
36+
trajectory = robot.plan_motion(goal_constraints,
37+
start_configuration,
38+
group,
39+
planner_id='RRT')
40+
41+
print("Computed kinematic path with %d configurations." % len(trajectory.points))
42+
print("Executing this path at full speed would take approx. %.3f seconds." % trajectory.time_from_start)

0 commit comments

Comments
 (0)