diff --git a/docs/examples/05_backends_pybullet/files/02_forward_kinematics.py b/docs/examples/05_backends_pybullet/files/02_forward_kinematics.py index 1bcfaad9a..b8ad255a0 100644 --- a/docs/examples/05_backends_pybullet/files/02_forward_kinematics.py +++ b/docs/examples/05_backends_pybullet/files/02_forward_kinematics.py @@ -22,8 +22,8 @@ # This is a simple robot cell with only the robot planner.set_robot_cell(robot_cell) - configuration = Configuration.from_revolute_values([-2.238, -1.153, -2.174, 0.185, 0.667, 0.0]) - robot_cell_state.robot_configuration = configuration + robot_cell_state.robot_configuration.joint_values = [-2.238, -1.153, -2.174, 0.185, 0.667, 0.0] + # In this demo, the default planning group is used for the forward kinematics frame_WCF = planner.forward_kinematics(robot_cell_state, TargetMode.ROBOT) @@ -36,4 +36,4 @@ # --------------------------------- for link_name in robot_cell.get_link_names(): frame_WCF = planner.forward_kinematics_to_link(robot_cell_state, link_name) - print("Frame of link '{}' : {}".format((link_name, frame_WCF))) + print("Frame of link '{}' : {}".format(link_name, frame_WCF)) diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py index 27f04f363..2223fefac 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py +++ b/src/compas_fab/backends/pybullet/backend_features/pybullet_forward_kinematics.py @@ -105,8 +105,8 @@ def forward_kinematics(self, robot_cell_state, target_mode, group=None, native_s return target_frame - def forward_kinematics_to_link(self, robot_cell_state, link_name=None, group=None, native_scale=None, options=None): - # type: (RobotCellState, Optional[str], Optional[str], Optional[float], Optional[dict]) -> Frame + def forward_kinematics_to_link(self, robot_cell_state, link_name=None, native_scale=None, options=None): + # type: (RobotCellState, Optional[str], Optional[float], Optional[dict]) -> Frame """Calculate the frame of the specified robot link from the provided RobotCellState. This function operates similar to :meth:`compas_fab.backends.PyBulletForwardKinematics.forward_kinematics`, @@ -123,9 +123,6 @@ def forward_kinematics_to_link(self, robot_cell_state, link_name=None, group=Non link_name : str, optional The name of the link to calculate the forward kinematics for. Defaults to the last link of the provided planning group. - group : str, optional - The planning group of the robot. - Defaults to the robot's main planning group. native_scale : float, optional The scaling factor to apply to the resulting frame. It is defined as `user_object_value * native_scale = meter_object_value`. @@ -139,11 +136,6 @@ def forward_kinematics_to_link(self, robot_cell_state, link_name=None, group=Non planner = self # type: PyBulletPlanner client = planner.client # type: PyBulletClient robot_cell = client.robot_cell # type: RobotCell - group = group or robot_cell.main_group_name - - # Check if the planning group is supported by the planner - if group not in robot_cell.group_names: - raise PlanningGroupNotExistsError("Planning group '{}' is not supported by PyBullet planner.".format(group)) # Setting the entire robot cell state, including the robot configuration planner.set_robot_cell_state(robot_cell_state)