Skip to content

Commit

Permalink
RobotCell.default_cell_state()
Browse files Browse the repository at this point in the history
  • Loading branch information
yck011522 committed Nov 5, 2024
1 parent 9a37950 commit 6546f4e
Show file tree
Hide file tree
Showing 12 changed files with 27 additions and 44 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -2,8 +2,6 @@

from compas_fab.backends import MoveItPlanner
from compas_fab.backends import RosClient
from compas_fab.robots import RobotCell
from compas_fab.robots import RobotCellState
from compas_fab.robots import ToolLibrary

with RosClient() as client:
Expand All @@ -18,7 +16,7 @@
gripper = ToolLibrary.kinematic_gripper()
robot_cell.tool_models[gripper.name] = gripper
# Attach the gripper to the robot
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()
robot_cell_state.set_tool_attached_to_group(
gripper.name,
robot_cell.main_group_name,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,6 @@
import compas_fab
from compas_fab.backends import RosClient
from compas_fab.backends import MoveItPlanner
from compas_fab.robots import RobotCell
from compas_fab.robots import RobotCellState
from compas_fab.robots import ToolLibrary
from compas_fab.robots import RigidBody

Expand All @@ -31,7 +29,7 @@
input("Press Enter to continue...")

# In order to see the tool attached it is necessary to update the robot_cell_state
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()
# Modify the tool state to attach the cone to the robot
robot_cell_state.tool_states["cone"].attached_to_group = robot_cell.main_group_name
robot_cell_state.tool_states["cone"].attachment_frame = Frame([0.0, 0.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0])
Expand Down Expand Up @@ -59,7 +57,7 @@
robot_cell.tool_models[gripper.name] = gripper
robot_cell.tool_models[cone_tool.name] = cone_tool
# Attach the gripper to the robot
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()
# The following function `set_tool_attached_to_group` is an alternative way than that in example 1
# It will also ensure that only one tool is attached to the specified group by removing others
robot_cell_state.set_tool_attached_to_group(
Expand Down
4 changes: 1 addition & 3 deletions docs/examples/03_backends_ros/files/03_forward_kinematics.py
Original file line number Diff line number Diff line change
@@ -1,5 +1,3 @@
from compas_robots import Configuration
from compas_fab.robots import RobotCellState
from compas_fab.robots import TargetMode
from compas_fab.backends import RosClient
from compas_fab.backends import MoveItPlanner
Expand All @@ -9,7 +7,7 @@
assert robot_cell.robot_model.name == "ur5_robot"
planner = MoveItPlanner(client)

robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()
robot_cell_state.robot_configuration.joint_values = [-2.238, -1.153, -2.174, 0.185, 0.667, 0.0]

# When using the forward_kinematics() method with TargetMode.ROBOT, the last link of the robot's main group is used.
Expand Down
3 changes: 1 addition & 2 deletions docs/examples/03_backends_ros/files/03_inverse_kinematics.py
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
from compas_fab.backends import MoveItPlanner

from compas_fab.robots import FrameTarget
from compas_fab.robots import RobotCellState
from compas_fab.robots import TargetMode

with RosClient() as client:
Expand All @@ -12,7 +11,7 @@
planner = MoveItPlanner(client)

# Create a default RobotCellState from the RobotCell as the starting state
start_state = RobotCellState.from_robot_cell(robot_cell)
start_state = robot_cell.default_cell_state()

# Create a target frame with the ROBOT mode
frame_WCF = Frame([0.3, 0.1, 0.5], [0, -1, 0], [0, 0, -1])
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,6 @@
from compas_fab.backends import MoveItPlanner

from compas_fab.robots import FrameTarget
from compas_fab.robots import RobotCellState
from compas_fab.robots import TargetMode

with RosClient() as client:
Expand All @@ -14,7 +13,7 @@
frame_WCF = Frame([0.3, 0.1, 0.5], [0, -1, 0], [0, 0, -1])
target = FrameTarget(frame_WCF, TargetMode.ROBOT)

start_state = RobotCellState.from_robot_cell(robot_cell)
start_state = robot_cell.default_cell_state()

result_count = 0
for config in planner.iter_inverse_kinematics(target, start_state, options=dict(max_results=10)):
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,13 +4,11 @@
from compas_fab.backends import MoveItPlanner
from compas_fab.robots import FrameWaypoints
from compas_fab.robots import TargetMode
from compas_fab.robots import RobotCell
from compas_fab.robots import RobotCellState

with RosClient() as client:
planner = MoveItPlanner(client)
robot_cell = client.load_robot_cell()
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()

assert robot_cell.robot_model.name == "ur5_robot"

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -6,10 +6,8 @@
from math import pi
from compas_fab.backends import RosClient
from compas_fab.backends import MoveItPlanner
from compas_fab.robots import RobotCell
from compas_fab.robots import RigidBody
from compas_fab.robots import ToolLibrary
from compas_fab.robots import RobotCellState
from compas_fab.robots import FrameWaypoints
from compas_fab.robots import TargetMode
from compas_fab.robots import FrameTarget
Expand Down Expand Up @@ -51,7 +49,7 @@
# =====================================

# Attach the gripper to the robot
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()
robot_cell_state.set_tool_attached_to_group(
gripper.name,
robot_cell.main_group_name,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -7,9 +7,6 @@
from compas_fab.backends import AnalyticalKinematicsPlanner
from compas_fab.backends.kinematics.solvers import UR5Kinematics

from compas_fab.robots import RobotCell
from compas_fab.robots import RobotCellState

# Not loading the robot's geometry because AnalyticalKinematicsPlanner does not use it for collision checking
robot_cell, robot_cell_state = RobotCellLibrary.ur5(load_geometry=False)

Expand All @@ -24,7 +21,7 @@
target = FrameTarget(frame_WCF, TargetMode.ROBOT)

# The start state is not important here because there is no tools involved
start_state = RobotCellState.from_robot_cell(robot_cell)
start_state = robot_cell.default_cell_state()

# iter_inverse_kinematics() will return a generator that would yield possible IK solutions
print("\nResults of iter_inverse_kinematics():")
Expand Down
3 changes: 1 addition & 2 deletions src/compas_fab/backends/ros/client.py
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,6 @@
from compas_fab.backends.tasks import CancellableFutureResult
from compas_fab.robots import RobotSemantics
from compas_fab.robots import RobotCell
from compas_fab.robots import RobotCellState

from compas import IPY

Expand Down Expand Up @@ -217,7 +216,7 @@ def load_robot_cell(
model.load_geometry(loader, precision=precision)

self._robot_cell = RobotCell(robot_model=model, robot_semantics=semantics)
self._robot_cell_state = RobotCellState.from_robot_cell(self._robot_cell)
self._robot_cell_state = self._robot_cell.default_cell_state()

return self._robot_cell

Expand Down
4 changes: 2 additions & 2 deletions src/compas_fab/robots/robot_cell.py
Original file line number Diff line number Diff line change
Expand Up @@ -809,13 +809,13 @@ def fill_configuration_with_joint_names(self, configuration, group=None):
def default_cell_state(self):
"""Create a default robot cell state for the robot cell.
Equivalent to :meth:`RobotCellState.from_robot_cell` with the robot cell as input.
The robot's base frame will be assumed to be at worldXY frame.
All tools will be assumed to be in their zero configuration and positioned at worldXY frame.
All workpieces will be assumed to be in their base frame and not attached to any tool or link.
All tools and workpieces are assumed to be visible in the scene (is_hidden=False).
Equivalent to :meth:`RobotCellState.from_robot_cell` with the robot cell as input.
Returns
-------
:class:`~compas_fab.robots.RobotCellState`
Expand Down
25 changes: 12 additions & 13 deletions src/compas_fab/robots/robot_library.py
Original file line number Diff line number Diff line change
Expand Up @@ -13,7 +13,6 @@
import compas_fab
from compas_fab.robots import RigidBody
from compas_fab.robots import RobotCell
from compas_fab.robots import RobotCellState

if not IPY:
from typing import TYPE_CHECKING
Expand All @@ -22,7 +21,7 @@
from typing import Optional # noqa: F401
from typing import Tuple # noqa: F401

from compas_fab.backends.interfaces import ClientInterface # noqa: F401
from compas_fab.robots import RobotCellState # noqa: F401


__all__ = [
Expand Down Expand Up @@ -367,7 +366,7 @@ def rfl(cls, load_geometry=True):
srdf_filename=compas_fab.get("robot_library/rfl/robot_description_semantic.srdf"),
local_package_mesh_folder="robot_library/rfl" if load_geometry else None,
)
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()

return robot_cell, robot_cell_state

Expand Down Expand Up @@ -398,7 +397,7 @@ def ur5(cls, load_geometry=True):
srdf_filename=compas_fab.get("robot_library/ur5_robot/robot_description_semantic.srdf"),
local_package_mesh_folder="robot_library/ur5_robot" if load_geometry else None,
)
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()

# Remove the planning group called 'endeffector' because it only has one link and no joints.
del robot_cell.robot_semantics.groups["endeffector"]
Expand Down Expand Up @@ -429,7 +428,7 @@ def ur10e(cls, load_geometry=True):
srdf_filename=compas_fab.get("robot_library/ur10e_robot/robot_description_semantic.srdf"),
local_package_mesh_folder="robot_library/ur10e_robot" if load_geometry else None,
)
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()

# Remove the planning group called 'endeffector' because it only has one link and no joints.
del robot_cell.robot_semantics.groups["endeffector"]
Expand Down Expand Up @@ -460,7 +459,7 @@ def abb_irb4600_40_255(cls, load_geometry=True):
srdf_filename=compas_fab.get("robot_library/abb_irb4600_40_255/robot_description_semantic.srdf"),
local_package_mesh_folder="robot_library/abb_irb4600_40_255" if load_geometry else None,
)
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()

return robot_cell, robot_cell_state

Expand Down Expand Up @@ -488,7 +487,7 @@ def abb_irb120_3_58(cls, load_geometry=True):
srdf_filename=compas_fab.get("robot_library/abb_irb120_3_58/robot_description_semantic.srdf"),
local_package_mesh_folder="robot_library/abb_irb120_3_58" if load_geometry else None,
)
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()

return robot_cell, robot_cell_state

Expand Down Expand Up @@ -527,7 +526,7 @@ def panda(cls, load_geometry=True):
robot_model.remove_link(link.name)
robot_model.remove_joint(link.parent_joint.name)

robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()

# Remove the planning group called 'panda_hand' because it it not a "serial chain",
# the bifurcation of the chain into two fingers is not supported by planning backends.
Expand Down Expand Up @@ -586,7 +585,7 @@ def ur5_cone_tool(cls, load_geometry=True):
# ------------------------------------------------------------------------
# Re-Create RobotCellState after modifying the RobotCell
# ------------------------------------------------------------------------
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()

# Attach the tool to the robot's main group
touch_links = ["wrist_3_link"]
Expand Down Expand Up @@ -660,7 +659,7 @@ def ur5_gripper_one_beam(cls, load_geometry=True):
# ------------------------------------------------------------------------
# Re-Create RobotCellState after modifying the RobotCell
# ------------------------------------------------------------------------
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()

# ------------------------------------------------------------------------
# Tool Attachment
Expand Down Expand Up @@ -755,7 +754,7 @@ def abb_irb4600_40_255_gripper_one_beam(cls, load_geometry=True):
# ------------------------------------------------------------------------
# Re-Create RobotCellState after modifying the RobotCell
# ------------------------------------------------------------------------
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()

# Attach the tool to the robot's main group
attachment_frame = Frame([0.0, 0.0, 0.0], [0.0, 0.0, 1.0], [1.0, 0.0, 0.0])
Expand Down Expand Up @@ -822,7 +821,7 @@ def ur10e_gripper_one_beam(cls, load_geometry=True):
# ------------------------------------------------------------------------
# Re-Create RobotCellState after modifying the RobotCell
# ------------------------------------------------------------------------
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()

# ------------------------------------------------------------------------
# Tool Attachment
Expand Down Expand Up @@ -908,7 +907,7 @@ def abb_irb4600_40_255_printing_tool(cls, load_geometry=True):
# ------------------------------------------------------------------------
# Re-Create RobotCellState after modifying the RobotCell
# ------------------------------------------------------------------------
robot_cell_state = RobotCellState.from_robot_cell(robot_cell)
robot_cell_state = robot_cell.default_cell_state()

# ------------------------------------------------------------------------
# Tool Attachment
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -44,7 +44,7 @@ def test_forward_kinematics(ur5_planner_robot_only):
planner = ur5_planner_robot_only # type: AnalyticalKinematicsPlanner

# Initial state of the robot cell is the home position
start_state = RobotCellState.from_robot_cell(planner.robot_cell)
start_state = planner.robot_cell.default_cell_state()
frame = planner.forward_kinematics(start_state, TargetMode.ROBOT, group=None)

correct = Frame(
Expand All @@ -64,7 +64,7 @@ def test_iter_inverse_kinematics(ur5_planner_robot_only):
)

# The `iter_inverse_kinematics` method will return an iterator that yields all possible solutions
start_state = RobotCellState.from_robot_cell(planner.robot_cell)
start_state = planner.robot_cell.default_cell_state()
solutions = list(planner.iter_inverse_kinematics(target, start_state, group=None))
assert len(solutions) == 8
configuration = solutions[0]
Expand All @@ -77,7 +77,7 @@ def test_inverse_kinematics(ur5_planner_robot_only):
target = FrameTarget(
Frame((0.381, 0.093, 0.382), (0.371, -0.292, -0.882), (0.113, 0.956, -0.269)), TargetMode.ROBOT
)
start_state = RobotCellState.from_robot_cell(planner.robot_cell)
start_state = planner.robot_cell.default_cell_state()

# Test to confirm inverse_kinematics() return one solution at a time
solutions = [] # type: list[Configuration]
Expand Down

0 comments on commit 6546f4e

Please sign in to comment.