Skip to content

Commit

Permalink
Removed all cached_robot_model
Browse files Browse the repository at this point in the history
  • Loading branch information
yck011522 committed Jul 20, 2024
1 parent 7974c13 commit 177e672
Show file tree
Hide file tree
Showing 17 changed files with 224 additions and 742 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@
from compas_fab.backends import PyBulletPlanner
from compas_fab.robots import RobotCell
from compas_fab.robots import RobotCellState
from compas_fab.robots import RobotLibrary


# #############################################
# Headless (no-GUI) forwards kinematics example
Expand All @@ -12,8 +14,7 @@
# 'direct' mode
with PyBulletClient("direct") as client:
# The robot in this example is loaded from a URDF file
urdf_filename = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf")
robot = client.load_robot(urdf_filename)
robot = RobotLibrary.ur5()

# The planner object is needed to call the forward kinematics function
planner = PyBulletPlanner(client)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,14 @@
from compas_fab.backends import PyBulletPlanner

from compas_fab.robots import FrameTarget
from compas_fab.robots import RobotCell
from compas_fab.robots import RobotCellState
from compas_fab.robots import RobotLibrary

with PyBulletClient() as client:
urdf_filename = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf")
robot = client.load_robot(urdf_filename)
robot = RobotLibrary.ur5()
planner = PyBulletPlanner(client)
planner.set_robot_cell(RobotCell(robot))

frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
target = FrameTarget(frame_WCF)
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -4,12 +4,14 @@
from compas_fab.backends import PyBulletPlanner

from compas_fab.robots import FrameTarget
from compas_fab.robots import RobotCell
from compas_fab.robots import RobotCellState
from compas_fab.robots import RobotLibrary

with PyBulletClient(connection_type="direct") as client:
urdf_filename = compas_fab.get("robot_library/ur5_robot/urdf/robot_description.urdf")
robot = client.load_robot(urdf_filename)
robot = RobotLibrary.ur5()
planner = PyBulletPlanner(client)
planner.set_robot_cell(RobotCell(robot))

frame_WCF = Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0])
target = FrameTarget(frame_WCF)
Expand Down
3 changes: 0 additions & 3 deletions src/compas_fab/backends/interfaces/client.py
Original file line number Diff line number Diff line change
Expand Up @@ -25,9 +25,6 @@ class ClientInterface(object):
----------
robot : :class:`compas_fab.robots.Robot`, read-only
The robot instance associated with the client.
Typically this is set by the backend client after it is initialized, using
`client.load_robot()`. It cannot be changed after it is set.
Consult the chosen backend client for how to set this attribute.
"""

def __init__(self):
Expand Down
14 changes: 0 additions & 14 deletions src/compas_fab/backends/pybullet/backend_features/__init__.py
Original file line number Diff line number Diff line change
Expand Up @@ -21,29 +21,15 @@
from __future__ import absolute_import


from compas_fab.backends.pybullet.backend_features.pybullet_add_attached_collision_mesh import (
PyBulletAddAttachedCollisionMesh,
)
from compas_fab.backends.pybullet.backend_features.pybullet_add_collision_mesh import PyBulletAddCollisionMesh
from compas_fab.backends.pybullet.backend_features.pybullet_append_collision_mesh import PyBulletAppendCollisionMesh
from compas_fab.backends.pybullet.backend_features.pybullet_forward_kinematics import PyBulletForwardKinematics
from compas_fab.backends.pybullet.backend_features.pybullet_inverse_kinematics import PyBulletInverseKinematics
from compas_fab.backends.pybullet.backend_features.pybullet_remove_attached_collision_mesh import (
PyBulletRemoveAttachedCollisionMesh,
)
from compas_fab.backends.pybullet.backend_features.pybullet_remove_collision_mesh import PyBulletRemoveCollisionMesh
from compas_fab.backends.pybullet.backend_features.pybullet_set_robot_cell import PyBulletSetRobotCell
from compas_fab.backends.pybullet.backend_features.pybullet_set_robot_cell_state import PyBulletSetRobotCellState


__all__ = [
"PyBulletAddAttachedCollisionMesh",
"PyBulletAddCollisionMesh",
"PyBulletAppendCollisionMesh",
"PyBulletForwardKinematics",
"PyBulletInverseKinematics",
"PyBulletRemoveAttachedCollisionMesh",
"PyBulletRemoveCollisionMesh",
"PyBulletSetRobotCell",
"PyBulletSetRobotCellState",
]

This file was deleted.

This file was deleted.

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -74,8 +74,6 @@ def forward_kinematics(self, robot_cell_state, group=None, options=None):
planner = self # type: PyBulletPlanner
robot = client.robot
group = group or robot.main_group_name
cached_robot_model = client.get_cached_robot_model(robot)
robot_body_id = client.get_uid(cached_robot_model)

# Setting the entire robot cell state, including the robot configuration
planner.set_robot_cell_state(robot_cell_state)
Expand All @@ -89,13 +87,13 @@ def forward_kinematics(self, robot_cell_state, group=None, options=None):
if link_name:
if link_name not in robot.get_link_names(group):
raise KeyError("Link name provided is not part of the group")
link_id = client._get_link_id_by_name(link_name, cached_robot_model)
fk_frame = client._get_link_frame(link_id, robot_body_id)
link_id = client.robot_link_puids[link_name]
fk_frame = client._get_link_frame(link_id, client.robot_puid)

else:
link_name = robot.get_end_effector_link_name(group)
link_id = client._get_link_id_by_name(link_name, cached_robot_model)
fk_frame = client._get_link_frame(link_id, robot_body_id)
link_id = client.robot_link_puids[link_name]
fk_frame = client._get_link_frame(link_id, client.robot_puid)

# If no link name provided, and a tool is attached to the group, return the tool tip frame of the tool
robot_cell = planner.robot_cell # type: RobotCell
Expand Down
Loading

0 comments on commit 177e672

Please sign in to comment.