From 5dca4e7b69992729d23599b2c97cb09628127e8b Mon Sep 17 00:00:00 2001 From: oeldardear Date: Tue, 19 Nov 2024 16:31:16 +0100 Subject: [PATCH 01/22] [TestMultiverse] Fix when multiverse is not found. --- test/test_mjcf.py | 2 ++ test/test_multiverse.py | 2 ++ 2 files changed, 4 insertions(+) diff --git a/test/test_mjcf.py b/test/test_mjcf.py index edfbb842b..6bad73bdc 100644 --- a/test/test_mjcf.py +++ b/test/test_mjcf.py @@ -12,6 +12,8 @@ class TestMjcf(TestCase): @classmethod def setUpClass(cls): + if MJCFObjDesc is None: + return # Example usage model = mjcf.RootElement("test") diff --git a/test/test_multiverse.py b/test/test_multiverse.py index d0e75ce91..e8be0322b 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -47,6 +47,8 @@ def setUpClass(cls): @classmethod def tearDownClass(cls): + if not multiverse_installed: + return cls.multiverse.exit(remove_saved_states=True) cls.multiverse.remove_multiverse_resources() From 6a15414c28280350866a488fd6586e02f72da477 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Thu, 21 Nov 2024 15:24:07 +0100 Subject: [PATCH 02/22] [adding iCub] first test for robot description --- .../robots/iCubGazeboV2_5_visuomanip.urdf | 3043 +++++++++++++++++ .../process_modules/iCub_process_modules.py | 281 ++ .../robot_descriptions/iCub_description.py | 79 + 3 files changed, 3403 insertions(+) create mode 100644 resources/robots/iCubGazeboV2_5_visuomanip.urdf create mode 100644 src/pycram/process_modules/iCub_process_modules.py create mode 100644 src/pycram/robot_descriptions/iCub_description.py diff --git a/resources/robots/iCubGazeboV2_5_visuomanip.urdf b/resources/robots/iCubGazeboV2_5_visuomanip.urdf new file mode 100644 index 000000000..3f3a6afbb --- /dev/null +++ b/resources/robots/iCubGazeboV2_5_visuomanip.urdf @@ -0,0 +1,3043 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 1 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + + true + + + true + + true + + + true + + true + + + true + + true + + + true + + true + + + true + + true + + + + 0 + 100000 + + 4 + 18000000 + 100 + 100 + 0.0001 + 1 + 1 + 0 0 0 + + + + 0 + 100000 + + 4 + 18000000 + 100 + 100 + 0.0001 + 1 + 1 + 0 0 0 + + + + 1 + 100 + 0.009500000190735 0.133444 0.009299999999999996 -1.5707963267948963 1.5707963267948963 0.0 + + model://iCub/conf/gazebo_icub_inertial.ini + + + + + + + + + + model://iCub/conf/gazebo_icub_torso.ini + + + model://iCub/conf/gazebo_icub_head_without_eyes.ini + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_eyes.ini + + + model://iCub/conf/gazebo_icub_left_arm_no_hand_for_no_hand_model.ini + -0.52 0.52 0 0.785 0 0 0.0 + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_hand_finger.ini + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_hand_thumb.ini + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_hand_index.ini + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_hand_middle.ini + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_hand_pinky.ini + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_hand_mais.ini + + + model://iCub/conf/gazebo_icub_right_arm_no_hand_for_no_hand_model.ini + -0.52 0.52 0 0.785 0 0 0.0 + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_hand_finger.ini + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_hand_thumb.ini + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_hand_index.ini + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_hand_middle.ini + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_hand_pinky.ini + + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_hand_mais.ini + +  + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/icub.xml + + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + 1 + + + + + + + 5.0 + 5.0 + + + + + + + + + + + 5.0 + 5.0 + + + + + + + + + + + 5.0 + 5.0 + + + + + + + + + + + 5.0 + 5.0 + + + + + + + + + + + 5.0 + 5.0 + + + + + + + + + + + 5.0 + 5.0 + + + + + + + + + + + 5.0 + 5.0 + + + + + + + + + + + 5.0 + 5.0 + + + + + + + + + + + 5.0 + 5.0 + + + + + + + + + + + 5.0 + 5.0 + + + + + + + + 0.0 0.0 0.0 0.0 -1.57 1.57 + + + + 0.8726646259971648 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + false + + + 0.0 0.0 0.0 0.0 -1.57 1.57 + + + + 0.8726646259971648 + + 320 + 240 + + + 0.1 + 100 + + + 1 + 30 + false + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_left_camera_rgbd.ini + + + + + + 0.0 0.0 0.0 0.0 -1.57 1.57 + + + + 0.8726646259971648 + + 320 + 240 + R8G8B8 + + + 0.1 + 100 + + + 1 + 30 + false + + model://iCub/conf_manual/iCubGazeboV2_5_visuomanip/gazebo_icub_right_camera_rgb.ini + + + + + 0.0 0.0 0.63 0.0 0.0 3.14159265359 + + diff --git a/src/pycram/process_modules/iCub_process_modules.py b/src/pycram/process_modules/iCub_process_modules.py new file mode 100644 index 000000000..052ca12da --- /dev/null +++ b/src/pycram/process_modules/iCub_process_modules.py @@ -0,0 +1,281 @@ +from threading import Lock +from typing_extensions import Any + +import actionlib + +from .. import world_reasoning as btr +import numpy as np + +from ..process_module import ProcessModule, ProcessModuleManager +from ..external_interfaces.ik import request_ik +from ..ros.logging import logdebug +from ..utils import _apply_ik +from ..local_transformer import LocalTransformer +from ..designators.object_designator import ObjectDesignatorDescription +from ..designators.motion_designator import MoveMotion, LookingMotion, \ + DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \ + MoveGripperMotion, OpeningMotion, ClosingMotion +from ..robot_description import RobotDescription +from ..datastructures.world import World +from ..world_concepts.world_object import Object +from ..datastructures.pose import Pose +from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType +from ..external_interfaces import giskard +from ..external_interfaces.robokudo import * + + + +class iCubNavigation(ProcessModule): + """ + The process module to move the robot from one position to another. + """ + + def _execute(self, desig: MoveMotion): + print("iCubNavigate") + + +class iCubMoveHead(ProcessModule): + """ + This process module moves the head to look at a specific point in the world coordinate frame. + This point can either be a position or an object. + """ + + def _execute(self, desig: LookingMotion): + print("iCub Move Head") + + +class iCubMoveGripper(ProcessModule): + """ + This process module controls the gripper of the robot. They can either be opened or closed. + Furthermore, it can only moved one gripper at a time. + """ + + def _execute(self, desig: MoveGripperMotion): + print("iCub Move Gripper") + + +class iCubDetecting(ProcessModule): + """ + This process module tries to detect an object with the given type. To be detected the object has to be in + the field of view of the robot. + """ + + def _execute(self, desig: DetectingMotion): + print("iCub Detect") + +class iCubMoveTCP(ProcessModule): + """ + This process moves the tool center point of either the right or the left arm. + """ + + def _execute(self, desig: MoveTCPMotion): + print("iCub Move TCP") + + +class iCubMoveArmJoints(ProcessModule): + """ + This process modules moves the joints of either the right or the left arm. The joint states can be given as + list that should be applied or a pre-defined position can be used, such as "parking" + """ + + def _execute(self, desig: MoveArmJointsMotion): + print("iCub Move Arm Joints") + +class iCubMoveJoints(ProcessModule): + """ + Process Module for generic joint movements, is not confined to the arms but can move any joint of the robot + """ + + def _execute(self, desig: MoveJointsMotion): + print("iCub Move Joints") + + +class iCubWorldStateDetecting(ProcessModule): + """ + This process module detectes an object even if it is not in the field of view of the robot. + """ + + def _execute(self, desig: WorldStateDetectingMotion): + print("iCub World State Detecting") + + +class iCubOpen(ProcessModule): + """ + Low-level implementation of opening a container in the simulation. Assumes the handle is already grasped. + """ + + def _execute(self, desig: OpeningMotion): + print("iCub Open") + + +class iCubClose(ProcessModule): + """ + Low-level implementation that lets the robot close a grasped container, in simulation + """ + + def _execute(self, desig: ClosingMotion): + print("iCub Close") + + + +########################################################### +########## Process Modules for the Real iCub ############## +########################################################### + + +class iCubNavigationReal(ProcessModule): + """ + Process module for the real PR2 that sends a cartesian goal to giskard to move the robot base + """ + + def _execute(self, designator: MoveMotion) -> Any: + print("iCub Navigate Real") + + +class iCubMoveHeadReal(ProcessModule): + """ + Process module for the real robot to move that such that it looks at the given position. Uses the same calculation + as the simulated one + """ + + def _execute(self, desig: LookingMotion): + print("iCub Move Head Real") + +class iCubDetectingReal(ProcessModule): + """ + Process Module for the real Pr2 that tries to detect an object fitting the given object description. Uses Robokudo + for perception of the environment. + """ + + def _execute(self, designator: DetectingMotion) -> Any: + print("iCub Detecting Real") + + +class iCubMoveTCPReal(ProcessModule): + """ + Moves the tool center point of the real PR2 while avoiding all collisions + """ + + def _execute(self, designator: MoveTCPMotion) -> Any: + print("iCub Move TCP Real") + + +class iCubMoveArmJointsReal(ProcessModule): + """ + Moves the arm joints of the real iCub to the given configuration while avoiding all collisions + """ + + def _execute(self, designator: MoveArmJointsMotion) -> Any: + print("iCub Move Arm Joints Real") + + +class iCubMoveJointsReal(ProcessModule): + """ + Moves any joint using giskard, avoids all collisions while doint this. + """ + + def _execute(self, designator: MoveJointsMotion) -> Any: + print("iCub Move Joints Real") + + +class iCubMoveGripperReal(ProcessModule): + """ + Opens or closes the gripper of the real PR2, gripper uses an action server for this instead of giskard + """ + + def _execute(self, designator: MoveGripperMotion) -> Any: + print("iCub Move Gripper Real") + + +class iCubOpenReal(ProcessModule): + """ + Tries to open an already grasped container + """ + + def _execute(self, designator: OpeningMotion) -> Any: + print("iCub Open Real") + + +class iCubCloseReal(ProcessModule): + """ + Tries to close an already grasped container + """ + + def _execute(self, designator: ClosingMotion) -> Any: + print("iCub Close Real") + + +class ICubManager(ProcessModuleManager): + + def __init__(self): + super().__init__("iCub") + self._navigate_lock = Lock() + self._looking_lock = Lock() + self._detecting_lock = Lock() + self._move_tcp_lock = Lock() + self._move_arm_joints_lock = Lock() + self._world_state_detecting_lock = Lock() + self._move_joints_lock = Lock() + self._move_gripper_lock = Lock() + self._open_lock = Lock() + self._close_lock = Lock() + + def navigate(self): + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: + print('Navigate iCub') + return iCubNavigation(self._navigate_lock) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: + return iCubNavigationReal(self._navigate_lock) + + def looking(self): + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: + return iCubMoveHead(self._looking_lock) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: + return iCubMoveHeadReal(self._looking_lock) + + def detecting(self): + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: + return iCubDetecting(self._detecting_lock) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: + return iCubDetectingReal(self._detecting_lock) + + def move_tcp(self): + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: + return iCubMoveTCP(self._move_tcp_lock) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: + return iCubMoveTCPReal(self._move_tcp_lock) + + def move_arm_joints(self): + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: + return iCubMoveArmJoints(self._move_arm_joints_lock) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: + return iCubMoveArmJointsReal(self._move_arm_joints_lock) + + def world_state_detecting(self): + if (ProcessModuleManager.execution_type == ExecutionType.SIMULATED or + ProcessModuleManager.execution_type == ExecutionType.REAL): + return iCubWorldStateDetecting(self._world_state_detecting_lock) + + def move_joints(self): + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: + return iCubMoveJoints(self._move_joints_lock) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: + return iCubMoveJointsReal(self._move_joints_lock) + + def move_gripper(self): + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: + return iCubMoveGripper(self._move_gripper_lock) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: + return iCubMoveGripperReal(self._move_gripper_lock) + + def open(self): + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: + return iCubOpen(self._open_lock) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: + return iCubOpenReal(self._open_lock) + + def close(self): + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: + return iCubClose(self._close_lock) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: + return iCubCloseReal(self._close_lock) diff --git a/src/pycram/robot_descriptions/iCub_description.py b/src/pycram/robot_descriptions/iCub_description.py new file mode 100644 index 000000000..4866a2f3b --- /dev/null +++ b/src/pycram/robot_descriptions/iCub_description.py @@ -0,0 +1,79 @@ +from ..datastructures.dataclasses import VirtualMobileBaseJoints +from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ + RobotDescriptionManager, CameraDescription +from ..datastructures.enums import Arms, Grasp, GripperState, GripperType +from ..ros.ros_tools import get_ros_package_path + +from ..helper import get_robot_mjcf_path + +filename = get_ros_package_path('pycram') + '/resources/robots/' + "iCubGazeboV2_5_visuomanip" + '.urdf' + +iCub_description = RobotDescription("iCub", + "root_link", + "torso_1", + "torso_pitch", + filename) + +################################## Left Arm ################################## +left_arm = KinematicChainDescription("left", "chest", "l_hand", + iCub_description.urdf_object, arm_type=Arms.LEFT) +left_arm.add_static_joint_states("park", {'l_shoulder_pitch': -80.0, + 'l_shoulder_roll': 70.0, + 'l_shoulder_yaw': 30.0, + 'l_elbow': 70.0, + 'l_wrist_prosup': -25.0, + 'l_wrist_pitch': 0.0, + 'l_wrist_yaw': 0.0}) + +iCub_description.add_kinematic_chain_description(left_arm) + + + +################################## Left Gripper ################################## +left_gripper = EndEffectorDescription("left_gripper", "l_hand", "l_hand", + iCub_description.urdf_object) + +left_arm.end_effector = left_gripper + + + +################################## Right Arm ################################## +right_arm = KinematicChainDescription("right", "chest", "r_hand", + iCub_description.urdf_object, arm_type=Arms.RIGHT) +right_arm.add_static_joint_states("park", {'r_shoulder_pitch': 80.0, + 'r_shoulder_roll': 70.0, + 'r_shoulder_yaw': 30.0, + 'r_elbow': 70.0, + 'r_wrist_prosup': -25.0, + 'r_wrist_pitch': 0.0, + 'r_wrist_yaw': 0.0}) + +iCub_description.add_kinematic_chain_description(right_arm) + + +################################## Right Gripper ################################## +right_gripper = EndEffectorDescription("right_gripper", "r_hand", "r_hand", + iCub_description.urdf_object) + + +right_arm.end_effector = right_gripper + + + +################################## Camera ################################## +camera = CameraDescription("left_camera", "eyes_tilt_frame", 1.27, + 1.60, 0.99483, 0.75049) +iCub_description.add_camera_description(camera) + +################################## Neck ################################## +iCub_description.add_kinematic_chain("neck", "neck_1", "head") + +################################# Grasps ################################## +iCub_description.add_grasp_orientations({Grasp.FRONT: [0, 0, 0, 1], + Grasp.LEFT: [0, 0, -1, 1], + Grasp.RIGHT: [0, 0, 1, 1], + Grasp.TOP: [0, 1, 0, 1]}) + +# Add to RobotDescriptionManager +rdm = RobotDescriptionManager() +rdm.register_description(iCub_description) From c08350260952b253b2d9751c165e1efaa22af0e3 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Tue, 26 Nov 2024 18:06:30 +0100 Subject: [PATCH 03/22] [development] first valid test on icub (looking) --- src/pycram/process_module.py | 8 +- src/pycram/process_modules/__init__.py | 2 + src/pycram/process_modules/iCubYarpTest.py | 0 .../process_modules/iCub_process_modules.py | 109 +++++++++++++++++- .../robot_descriptions/iCub_description.py | 7 +- test/test_icub.py | 14 +++ 6 files changed, 136 insertions(+), 4 deletions(-) create mode 100644 src/pycram/process_modules/iCubYarpTest.py create mode 100644 test/test_icub.py diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index 957d0f5e9..157c9b268 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -330,7 +330,7 @@ def place(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def looking(self) -> Type[ProcessModule]: + def plooking(self) -> Type[ProcessModule]: """ Get the Process Module for looking at a point with respect to the :py:attr:`~ProcessModuleManager.execution_type` @@ -419,3 +419,9 @@ def close(self) -> Type[ProcessModule]: """ raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") + + def exit(self)->None: + """ + Exit process module and disconnect from robot + """ + pass diff --git a/src/pycram/process_modules/__init__.py b/src/pycram/process_modules/__init__.py index caa3dfe41..adf8de219 100644 --- a/src/pycram/process_modules/__init__.py +++ b/src/pycram/process_modules/__init__.py @@ -4,6 +4,7 @@ from .hsrb_process_modules import HSRBManager from .default_process_modules import DefaultManager from .stretch_process_modules import StretchManager +from .iCub_process_modules import ICubManager from .tiago_process_modules import TiagoManager Pr2Manager() @@ -12,4 +13,5 @@ HSRBManager() DefaultManager() StretchManager() +ICubManager() TiagoManager() diff --git a/src/pycram/process_modules/iCubYarpTest.py b/src/pycram/process_modules/iCubYarpTest.py new file mode 100644 index 000000000..e69de29bb diff --git a/src/pycram/process_modules/iCub_process_modules.py b/src/pycram/process_modules/iCub_process_modules.py index 052ca12da..f41e6279a 100644 --- a/src/pycram/process_modules/iCub_process_modules.py +++ b/src/pycram/process_modules/iCub_process_modules.py @@ -1,4 +1,6 @@ from threading import Lock +from typing import Union + from typing_extensions import Any import actionlib @@ -24,6 +26,26 @@ from ..external_interfaces.robokudo import * +import yarp +ACK_VOCAB = yarp.createVocab32('a','c','k') +NO_ACK_VOCAB = yarp.createVocab32('n','a','c','k') + +def init_yarp_network(): + if not yarp.Network.checkNetwork(): + print("Unable to find a yarp server exiting ...") + return False + + yarp.Network.init() + return True + +def open_rpc_client_port(port_name): + handle_port: yarp.RpcClient = yarp.RpcClient() + if not handle_port.open(port_name): + print(f"Can't open the port %s correctly" % port_name) + return False , None + print(f"Port %s opened correctly" % port_name) + return True , handle_port + class iCubNavigation(ProcessModule): """ @@ -40,8 +62,43 @@ class iCubMoveHead(ProcessModule): This point can either be a position or an object. """ + def __init__(self,lock,cmd_port:yarp.RpcClient): + super().__init__(lock) + self.cmd_port = cmd_port + def _execute(self, desig: LookingMotion): print("iCub Move Head") + position_target = desig.target.pose.position + if self.cmd_port.getOutputCount(): + + yarp_bottle_msg: yarp.Bottle = yarp.Bottle() + yarp_bottle_reply: yarp.Bottle = yarp.Bottle() + yarp_bottle_msg.addVocab32('l', 'o', 'o', 'k') + yarp_bottle_msg.addVocab32('3', 'D') + + target_loc: yarp.Bottle = yarp_bottle_msg.addList() + target_loc.addFloat32(position_target.x) + target_loc.addFloat32(position_target.y) + target_loc.addFloat32(position_target.z) + print(f"command Ready to send to iCub") + self.cmd_port .write(yarp_bottle_msg, yarp_bottle_reply) + reply_vocab = yarp_bottle_reply.get(0).asVocab32() + + if reply_vocab == NO_ACK_VOCAB: + print("NO_ACK") + return False + elif reply_vocab == ACK_VOCAB: + print("ACK") + return True + else: + print("another reply") + return False + + + else: + print("port is not connected") + return False + class iCubMoveGripper(ProcessModule): @@ -219,6 +276,24 @@ def __init__(self): self._move_gripper_lock = Lock() self._open_lock = Lock() self._close_lock = Lock() + self.yarp_network_state = init_yarp_network() + + # yarp related + self.gaze_cmd_port_name = "/pycram/gaze/cmd:oi" + self.action_cmd_port_name = "/pycram/action/cmd:oi" + + self.gaze_client_port = None + self.action_client_port = None + + if self.yarp_network_state: + print("yarp network state detected") + self.config_yarp_ports() + self.connect_yarp_ports() + else: + print("yarp network state not detected") + + + def navigate(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: @@ -229,7 +304,7 @@ def navigate(self): def looking(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: - return iCubMoveHead(self._looking_lock) + return iCubMoveHead(self._looking_lock,self.gaze_client_port) elif ProcessModuleManager.execution_type == ExecutionType.REAL: return iCubMoveHeadReal(self._looking_lock) @@ -279,3 +354,35 @@ def close(self): return iCubClose(self._close_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: return iCubCloseReal(self._close_lock) + + + def config_yarp_ports(self)->bool: + suc, self.gaze_client_port = open_rpc_client_port(self.gaze_cmd_port_name) + suc2, self.action_client_port = open_rpc_client_port(self.action_cmd_port_name) + + return suc and suc2 + + def connect_yarp_ports(self)->bool: + gaze_connected = yarp.NetworkBase_connect(self.gaze_cmd_port_name, "/iKinGazeCtrl/rpc", "tcp") + action_connected = yarp.NetworkBase_connect(self.action_cmd_port_name, "/actionsRenderingEngine/cmd:io", "tcp") + + if not gaze_connected: + print("gaze control port couldn't connect") + + if not action_connected: + print("action port couldn't connect") + + return gaze_connected and action_connected + + + def exit(self): + self.disconnect_and_remove() + + + def disconnect_and_remove(self): + self.gaze_client_port .interrupt() + self.gaze_client_port .close() + + self.action_client_port.interrupt() + self.action_client_port.close() + diff --git a/src/pycram/robot_descriptions/iCub_description.py b/src/pycram/robot_descriptions/iCub_description.py index 4866a2f3b..3e477d515 100644 --- a/src/pycram/robot_descriptions/iCub_description.py +++ b/src/pycram/robot_descriptions/iCub_description.py @@ -5,14 +5,17 @@ from ..ros.ros_tools import get_ros_package_path from ..helper import get_robot_mjcf_path +import icub_models -filename = get_ros_package_path('pycram') + '/resources/robots/' + "iCubGazeboV2_5_visuomanip" + '.urdf' +#filename = get_ros_package_path('pycram') + '/resources/robots/' + "iCubGazeboV2_5_visuomanip" + '.urdf' + +RobotName = 'iCubGazeboV2_5_visuomanip' iCub_description = RobotDescription("iCub", "root_link", "torso_1", "torso_pitch", - filename) + icub_models.get_model_file(RobotName)) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left", "chest", "l_hand", diff --git a/test/test_icub.py b/test/test_icub.py new file mode 100644 index 000000000..5cfd1d64e --- /dev/null +++ b/test/test_icub.py @@ -0,0 +1,14 @@ +from pycram.datastructures.pose import Pose +from pycram.designators.motion_designator import LookingMotion +from pycram.process_module import ProcessModuleManager, simulated_robot +from pycram.robot_description import RobotDescription, RobotDescriptionManager + +rm = RobotDescriptionManager() +rm.load_description('iCub') + +with simulated_robot: + pm = ProcessModuleManager.get_manager() + + LookingMotion(Pose([-2, -2, 3])).perform() + + pm.exit() From 0632c6c4af679e73020f057242ec079e372a9be0 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Tue, 3 Dec 2024 19:09:23 +0100 Subject: [PATCH 04/22] [feature] icub processing module add move joints motion action --- launch/ik_and_description.launch | 6 + .../process_modules/iCub_process_modules.py | 315 ++++++++++++++++-- .../robot_descriptions/iCub_description.py | 3 +- test/testRandom.py | 50 +++ 4 files changed, 348 insertions(+), 26 deletions(-) create mode 100644 test/testRandom.py diff --git a/launch/ik_and_description.launch b/launch/ik_and_description.launch index 4484eaf86..6ffb31ed0 100644 --- a/launch/ik_and_description.launch +++ b/launch/ik_and_description.launch @@ -16,6 +16,12 @@ + + + + + Any: print("iCub Navigate Real") -class iCubMoveHeadReal(ProcessModule): - """ - Process module for the real robot to move that such that it looks at the given position. Uses the same calculation - as the simulated one - """ - - def _execute(self, desig: LookingMotion): - print("iCub Move Head Real") - class iCubDetectingReal(ProcessModule): """ Process Module for the real Pr2 that tries to detect an object fitting the given object description. Uses Robokudo @@ -279,12 +437,29 @@ def __init__(self): self.yarp_network_state = init_yarp_network() # yarp related + self.robot_name_yarp = "icubSim" self.gaze_cmd_port_name = "/pycram/gaze/cmd:oi" self.action_cmd_port_name = "/pycram/action/cmd:oi" + self.ctp_torso_client_port_name = "/pycram/ctp/torso:oi" + self.ctp_right_arm_client_port_name = "/pycram/ctp/right_arm:oi" + self.ctp_left_arm_client_port_name = "/pycram/ctp/left_arm:oi" + + self.state_torso_port_name = "/pycram/ctp/torso:i" + self.state_right_arm_port_name = "/pycram/ctp/right_arm:i" + self.state_left_arm_port_name = "/pycram/ctp/left_arm:i" + self.gaze_client_port = None self.action_client_port = None + self.ctp_torso_client_port = None + self.ctp_right_arm_client_port = None + self.ctp_left_arm_client_port = None + + self.state_torso_port = yarp.BufferedPortBottle() + self.state_right_arm_port = yarp.BufferedPortBottle() + self.state_left_arm_port = yarp.BufferedPortBottle() + if self.yarp_network_state: print("yarp network state detected") self.config_yarp_ports() @@ -295,6 +470,9 @@ def __init__(self): + + + def navigate(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: print('Navigate iCub') @@ -306,7 +484,7 @@ def looking(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return iCubMoveHead(self._looking_lock,self.gaze_client_port) elif ProcessModuleManager.execution_type == ExecutionType.REAL: - return iCubMoveHeadReal(self._looking_lock) + return iCubMoveHead(self._looking_lock,self.gaze_client_port) def detecting(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: @@ -316,7 +494,7 @@ def detecting(self): def move_tcp(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: - return iCubMoveTCP(self._move_tcp_lock) + return iCubMoveTCP(self._move_tcp_lock,self.action_client_port) elif ProcessModuleManager.execution_type == ExecutionType.REAL: return iCubMoveTCPReal(self._move_tcp_lock) @@ -333,7 +511,9 @@ def world_state_detecting(self): def move_joints(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: - return iCubMoveJoints(self._move_joints_lock) + return iCubMoveJoints(self._move_joints_lock, + [self.state_torso_port,self.state_right_arm_port,self.state_right_arm_port], + [self.ctp_torso_client_port,self.ctp_right_arm_client_port,self.ctp_right_arm_client_port]) elif ProcessModuleManager.execution_type == ExecutionType.REAL: return iCubMoveJointsReal(self._move_joints_lock) @@ -358,21 +538,93 @@ def close(self): def config_yarp_ports(self)->bool: suc, self.gaze_client_port = open_rpc_client_port(self.gaze_cmd_port_name) - suc2, self.action_client_port = open_rpc_client_port(self.action_cmd_port_name) + if not suc: + print("Failed to open, ", self.gaze_cmd_port_name) + return False + suc, self.action_client_port = open_rpc_client_port(self.action_cmd_port_name) + if not suc: + print("Failed to open, ", self.action_cmd_port_name) + return False - return suc and suc2 + suc, self.ctp_torso_client_port = open_rpc_client_port(self.ctp_torso_client_port_name) + if not suc: + print("Failed to open, ", self.ctp_torso_client_port_name) + return False - def connect_yarp_ports(self)->bool: - gaze_connected = yarp.NetworkBase_connect(self.gaze_cmd_port_name, "/iKinGazeCtrl/rpc", "tcp") - action_connected = yarp.NetworkBase_connect(self.action_cmd_port_name, "/actionsRenderingEngine/cmd:io", "tcp") + suc, self.ctp_right_arm_client_port = open_rpc_client_port(self.ctp_right_arm_client_port_name) + if not suc: + print("Failed to open, ", self.ctp_right_arm_client_port_name) + return False + + suc, self.ctp_left_arm_client_port = open_rpc_client_port(self.ctp_left_arm_client_port_name) + if not suc: + print("Failed to open, ", self.ctp_left_arm_client_port_name) + return False - if not gaze_connected: + suc, self.state_torso_port = open_buffered_bottle_port(self.state_torso_port_name) + if not suc: + print("Failed to open, ", self.state_torso_port_name) + return False + + suc, self.state_right_arm_port = open_buffered_bottle_port(self.state_right_arm_port_name) + if not suc: + print("Failed to open, ", self.state_right_arm_port_name) + return False + + suc, self.state_left_arm_port = open_buffered_bottle_port(self.state_left_arm_port_name) + if not suc: + print("Failed to open, ", self.state_left_arm_port_name) + return False + + + + return True + + def connect_yarp_ports(self)->bool: + connection_status = yarp.NetworkBase_connect(self.gaze_cmd_port_name, "/iKinGazeCtrl/rpc", "tcp") + if not connection_status: print("gaze control port couldn't connect") - if not action_connected: - print("action port couldn't connect") + connection_status = yarp.NetworkBase_connect(self.action_cmd_port_name, "/actionsRenderingEngine/cmd:io", "tcp") + if not connection_status: + print("action control port couldn't connect") + return False - return gaze_connected and action_connected + # ctp service ports + connection_status = yarp.NetworkBase_connect(self.ctp_torso_client_port_name, "/ctpservice/torso/rpc", "tcp") + #connection_status = yarp.NetworkBase_connect(self.ctp_torso_client_port_name, "/testrpc", "tcp") + + if not connection_status: + print("action control port couldn't connect") + return False + + connection_status = yarp.NetworkBase_connect(self.ctp_right_arm_client_port_name, "/ctpservice/right_arm/rpc", "tcp") + if not connection_status: + print("action control port couldn't connect") + return False + + connection_status = yarp.NetworkBase_connect( self.ctp_left_arm_client_port_name,"/ctpservice/left_arm/rpc", "tcp") + if not connection_status: + print("action control port couldn't connect") + return False + + # status ports + connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/torso/state:o", self.state_torso_port_name, "tcp") + if not connection_status: + print("action control port couldn't connect") + return False + + connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/right_arm/state:o",self.state_right_arm_port_name, "tcp") + if not connection_status: + print("action control port couldn't connect") + return False + + connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/left_arm/state:o",self.state_left_arm_port_name , "tcp") + if not connection_status: + print("action control port couldn't connect") + return False + + return True def exit(self): @@ -381,8 +633,21 @@ def exit(self): def disconnect_and_remove(self): self.gaze_client_port .interrupt() - self.gaze_client_port .close() - self.action_client_port.interrupt() + self.ctp_torso_client_port.interrupt() + self.ctp_right_arm_client_port.interrupt() + self.ctp_left_arm_client_port.interrupt() + self.state_torso_port.interrupt() + self.state_right_arm_port.interrupt() + self.state_left_arm_port.interrupt() + + self.gaze_client_port .close() self.action_client_port.close() + self.ctp_torso_client_port.close() + self.ctp_right_arm_client_port.close() + self.ctp_left_arm_client_port.close() + self.state_torso_port.close() + self.state_right_arm_port.close() + self.state_left_arm_port.close() + diff --git a/src/pycram/robot_descriptions/iCub_description.py b/src/pycram/robot_descriptions/iCub_description.py index 3e477d515..357f26549 100644 --- a/src/pycram/robot_descriptions/iCub_description.py +++ b/src/pycram/robot_descriptions/iCub_description.py @@ -8,6 +8,7 @@ import icub_models #filename = get_ros_package_path('pycram') + '/resources/robots/' + "iCubGazeboV2_5_visuomanip" + '.urdf' +filename_ros_package = "/usr/local/src/robot/Bremen/workspace/ros/src/icub_model/urdf/model.urdf" RobotName = 'iCubGazeboV2_5_visuomanip' @@ -15,7 +16,7 @@ "root_link", "torso_1", "torso_pitch", - icub_models.get_model_file(RobotName)) + filename_ros_package) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left", "chest", "l_hand", diff --git a/test/testRandom.py b/test/testRandom.py new file mode 100644 index 000000000..2cf9378bb --- /dev/null +++ b/test/testRandom.py @@ -0,0 +1,50 @@ +from pycram.datastructures.pose import Pose +from pycram.description import ObjectDescription +from pycram.designators.motion_designator import LookingMotion, MoveTCPMotion +from pycram.process_module import ProcessModuleManager, simulated_robot +from pycram.robot_description import RobotDescription, RobotDescriptionManager +from pycram.datastructures.enums import GripperState, Arms, WorldMode +from pycram.worlds.bullet_world import BulletWorld +from pycram.world_concepts.world_object import Object +from pycram.datastructures.enums import ObjectType, WorldMode +from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher +from pycram.worlds.bullet_world import BulletWorld +from pycram.designators.action_designator import * +from pycram.designators.location_designator import * +from pycram.designators.object_designator import * +from pycram.datastructures.enums import ObjectType, WorldMode +from pycram.datastructures.pose import Pose +from pycram.process_module import simulated_robot, with_simulated_robot +from pycram.object_descriptors.urdf import ObjectDescription +from pycram.world_concepts.world_object import Object +from pycram.datastructures.dataclasses import Color +from pycram.designators.object_designator import BelieveObject + +def run_look_and_move_tcp(): + rm = RobotDescriptionManager() + rm.load_description('iCub') + + with simulated_robot: + pm = ProcessModuleManager.get_manager() + + LookingMotion(Pose([-2, -2, 3])).perform() + time.sleep(2) + MoveTCPMotion(Pose([-0.2, 0.1, 0.1]), Arms.LEFT).perform() + MoveJointsMotion(["torso_roll"],[20.0]).perform() + + pm.exit() + +def try_world(): + extension: str = ObjectDescription.get_file_extension() + rdm = RobotDescriptionManager() + rdm.load_description("pr2") + world = BulletWorld(mode=WorldMode.DIRECT) + milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) + + cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", + pose=Pose([1.3, 0.7, 0.95])) + + + + +run_look_and_move_tcp() From be79bf2b88bd8482cf758370bc38631910c9c55d Mon Sep 17 00:00:00 2001 From: oeldardear Date: Wed, 4 Dec 2024 13:29:48 +0100 Subject: [PATCH 05/22] [feature] first test icub on pybullet --- ...ubGazeboV2_5_visuomanip.urdf => icub.urdf} | 316 +++++++++--------- src/pycram/process_module.py | 2 +- src/pycram/process_modules/__init__.py | 2 +- ...ess_modules.py => icub_process_modules.py} | 8 +- ...Cub_description.py => icub_description.py} | 34 +- src/pycram/world_concepts/world_object.py | 6 +- test/bullet_world_testcase.py | 0 test/testRandom.py | 40 ++- 8 files changed, 214 insertions(+), 194 deletions(-) rename resources/robots/{iCubGazeboV2_5_visuomanip.urdf => icub.urdf} (85%) rename src/pycram/process_modules/{iCub_process_modules.py => icub_process_modules.py} (99%) rename src/pycram/robot_descriptions/{iCub_description.py => icub_description.py} (69%) create mode 100644 test/bullet_world_testcase.py diff --git a/resources/robots/iCubGazeboV2_5_visuomanip.urdf b/resources/robots/icub.urdf similarity index 85% rename from resources/robots/iCubGazeboV2_5_visuomanip.urdf rename to resources/robots/icub.urdf index 3f3a6afbb..e3ebc52dc 100644 --- a/resources/robots/iCubGazeboV2_5_visuomanip.urdf +++ b/resources/robots/icub.urdf @@ -9,7 +9,7 @@ - + @@ -18,7 +18,7 @@ - + @@ -36,7 +36,7 @@ - + @@ -45,7 +45,7 @@ - + @@ -66,7 +66,7 @@ - + @@ -75,7 +75,7 @@ - + @@ -96,7 +96,7 @@ - + @@ -105,7 +105,7 @@ - + @@ -124,7 +124,7 @@ - + @@ -133,7 +133,7 @@ - + @@ -161,7 +161,7 @@ - + @@ -170,7 +170,7 @@ - + @@ -191,7 +191,7 @@ - + @@ -200,7 +200,7 @@ - + @@ -221,7 +221,7 @@ - + @@ -230,7 +230,7 @@ - + @@ -251,7 +251,7 @@ - + @@ -279,7 +279,7 @@ - + @@ -288,7 +288,7 @@ - + @@ -309,7 +309,7 @@ - + @@ -318,7 +318,7 @@ - + @@ -339,7 +339,7 @@ - + @@ -348,7 +348,7 @@ - + @@ -367,7 +367,7 @@ - + @@ -376,7 +376,7 @@ - + @@ -404,7 +404,7 @@ - + @@ -413,7 +413,7 @@ - + @@ -434,7 +434,7 @@ - + @@ -443,7 +443,7 @@ - + @@ -464,7 +464,7 @@ - + @@ -473,7 +473,7 @@ - + @@ -494,7 +494,7 @@ - + @@ -522,7 +522,7 @@ - + @@ -531,7 +531,7 @@ - + @@ -552,7 +552,7 @@ - + @@ -561,7 +561,7 @@ - + @@ -582,7 +582,7 @@ - + @@ -591,7 +591,7 @@ - + @@ -612,7 +612,7 @@ - + @@ -621,7 +621,7 @@ - + @@ -642,7 +642,7 @@ - + @@ -651,7 +651,7 @@ - + @@ -672,7 +672,7 @@ - + @@ -681,7 +681,7 @@ - + @@ -702,7 +702,7 @@ - + @@ -711,7 +711,7 @@ - + @@ -730,7 +730,7 @@ - + @@ -739,7 +739,7 @@ - + @@ -760,7 +760,7 @@ - + @@ -769,7 +769,7 @@ - + @@ -797,7 +797,7 @@ - + @@ -806,7 +806,7 @@ - + @@ -827,7 +827,7 @@ - + @@ -836,7 +836,7 @@ - + @@ -845,7 +845,7 @@ - + @@ -873,7 +873,7 @@ - + @@ -883,7 +883,7 @@ 1 - + @@ -904,7 +904,7 @@ - + @@ -914,7 +914,7 @@ 1 - + @@ -935,7 +935,7 @@ - + @@ -945,7 +945,7 @@ 1 - + @@ -966,7 +966,7 @@ - + @@ -976,7 +976,7 @@ 1 - + @@ -1004,7 +1004,7 @@ - + @@ -1014,7 +1014,7 @@ 1 - + @@ -1035,7 +1035,7 @@ - + @@ -1045,7 +1045,7 @@ 1 - + @@ -1066,7 +1066,7 @@ - + @@ -1076,7 +1076,7 @@ 1 - + @@ -1097,7 +1097,7 @@ - + @@ -1107,7 +1107,7 @@ 1 - + @@ -1135,7 +1135,7 @@ - + @@ -1145,7 +1145,7 @@ 1 - + @@ -1166,7 +1166,7 @@ - + @@ -1176,7 +1176,7 @@ 1 - + @@ -1197,7 +1197,7 @@ - + @@ -1207,7 +1207,7 @@ 1 - + @@ -1228,7 +1228,7 @@ - + @@ -1238,7 +1238,7 @@ 1 - + @@ -1266,7 +1266,7 @@ - + @@ -1276,7 +1276,7 @@ 1 - + @@ -1297,7 +1297,7 @@ - + @@ -1307,7 +1307,7 @@ 1 - + @@ -1328,7 +1328,7 @@ - + @@ -1338,7 +1338,7 @@ 1 - + @@ -1359,7 +1359,7 @@ - + @@ -1369,7 +1369,7 @@ 1 - + @@ -1397,7 +1397,7 @@ - + @@ -1407,7 +1407,7 @@ 1 - + @@ -1428,7 +1428,7 @@ - + @@ -1438,7 +1438,7 @@ 1 - + @@ -1459,7 +1459,7 @@ - + @@ -1469,7 +1469,7 @@ 1 - + @@ -1490,7 +1490,7 @@ - + @@ -1500,7 +1500,7 @@ 1 - + @@ -1528,7 +1528,7 @@ - + @@ -1537,7 +1537,7 @@ - + @@ -1558,7 +1558,7 @@ - + @@ -1567,7 +1567,7 @@ - + @@ -1588,7 +1588,7 @@ - + @@ -1597,7 +1597,7 @@ - + @@ -1618,7 +1618,7 @@ - + @@ -1627,7 +1627,7 @@ - + @@ -1646,7 +1646,7 @@ - + @@ -1655,7 +1655,7 @@ - + @@ -1676,7 +1676,7 @@ - + @@ -1685,7 +1685,7 @@ - + @@ -1713,7 +1713,7 @@ - + @@ -1722,7 +1722,7 @@ - + @@ -1743,7 +1743,7 @@ - + @@ -1752,7 +1752,7 @@ - + @@ -1761,7 +1761,7 @@ - + @@ -1789,7 +1789,7 @@ - + @@ -1799,7 +1799,7 @@ 1 - + @@ -1820,7 +1820,7 @@ - + @@ -1830,7 +1830,7 @@ 1 - + @@ -1851,7 +1851,7 @@ - + @@ -1861,7 +1861,7 @@ 1 - + @@ -1882,7 +1882,7 @@ - + @@ -1892,7 +1892,7 @@ 1 - + @@ -1920,7 +1920,7 @@ - + @@ -1930,7 +1930,7 @@ 1 - + @@ -1951,7 +1951,7 @@ - + @@ -1961,7 +1961,7 @@ 1 - + @@ -1982,7 +1982,7 @@ - + @@ -1992,7 +1992,7 @@ 1 - + @@ -2013,7 +2013,7 @@ - + @@ -2023,7 +2023,7 @@ 1 - + @@ -2051,7 +2051,7 @@ - + @@ -2060,7 +2060,7 @@ - + @@ -2081,7 +2081,7 @@ - + @@ -2091,7 +2091,7 @@ 1 - + @@ -2112,7 +2112,7 @@ - + @@ -2122,7 +2122,7 @@ 1 - + @@ -2143,7 +2143,7 @@ - + @@ -2153,7 +2153,7 @@ 1 - + @@ -2181,7 +2181,7 @@ - + @@ -2191,7 +2191,7 @@ 1 - + @@ -2212,7 +2212,7 @@ - + @@ -2222,7 +2222,7 @@ 1 - + @@ -2243,7 +2243,7 @@ - + @@ -2253,7 +2253,7 @@ 1 - + @@ -2274,7 +2274,7 @@ - + @@ -2284,7 +2284,7 @@ 1 - + @@ -2312,7 +2312,7 @@ - + @@ -2322,7 +2322,7 @@ 1 - + @@ -2343,7 +2343,7 @@ - + @@ -2353,7 +2353,7 @@ 1 - + @@ -2374,7 +2374,7 @@ - + @@ -2384,7 +2384,7 @@ 1 - + @@ -2405,7 +2405,7 @@ - + @@ -2415,7 +2415,7 @@ 1 - + @@ -2443,7 +2443,7 @@ - + @@ -2452,7 +2452,7 @@ - + @@ -2473,7 +2473,7 @@ - + @@ -2482,7 +2482,7 @@ - + @@ -2503,7 +2503,7 @@ - + @@ -2512,7 +2512,7 @@ - + diff --git a/src/pycram/process_module.py b/src/pycram/process_module.py index 157c9b268..b07a5e212 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -330,7 +330,7 @@ def place(self) -> Type[ProcessModule]: raise NotImplementedError( f"There are no Process Modules for '{inspect.currentframe().f_code.co_name}' for robot '{self.robot_name}'") - def plooking(self) -> Type[ProcessModule]: + def looking(self) -> Type[ProcessModule]: """ Get the Process Module for looking at a point with respect to the :py:attr:`~ProcessModuleManager.execution_type` diff --git a/src/pycram/process_modules/__init__.py b/src/pycram/process_modules/__init__.py index adf8de219..a084c31e4 100644 --- a/src/pycram/process_modules/__init__.py +++ b/src/pycram/process_modules/__init__.py @@ -4,8 +4,8 @@ from .hsrb_process_modules import HSRBManager from .default_process_modules import DefaultManager from .stretch_process_modules import StretchManager -from .iCub_process_modules import ICubManager from .tiago_process_modules import TiagoManager +from .icub_process_modules import ICubManager Pr2Manager() BoxyManager() diff --git a/src/pycram/process_modules/iCub_process_modules.py b/src/pycram/process_modules/icub_process_modules.py similarity index 99% rename from src/pycram/process_modules/iCub_process_modules.py rename to src/pycram/process_modules/icub_process_modules.py index ac8fce912..8c2868c42 100644 --- a/src/pycram/process_modules/iCub_process_modules.py +++ b/src/pycram/process_modules/icub_process_modules.py @@ -423,7 +423,7 @@ def _execute(self, designator: ClosingMotion) -> Any: class ICubManager(ProcessModuleManager): def __init__(self): - super().__init__("iCub") + super().__init__("icub") self._navigate_lock = Lock() self._looking_lock = Lock() self._detecting_lock = Lock() @@ -459,11 +459,12 @@ def __init__(self): self.state_torso_port = yarp.BufferedPortBottle() self.state_right_arm_port = yarp.BufferedPortBottle() self.state_left_arm_port = yarp.BufferedPortBottle() - + self.initialized = False if self.yarp_network_state: print("yarp network state detected") self.config_yarp_ports() self.connect_yarp_ports() + self.initialized = True else: print("yarp network state not detected") @@ -628,7 +629,8 @@ def connect_yarp_ports(self)->bool: def exit(self): - self.disconnect_and_remove() + if self.initialized: + self.disconnect_and_remove() def disconnect_and_remove(self): diff --git a/src/pycram/robot_descriptions/iCub_description.py b/src/pycram/robot_descriptions/icub_description.py similarity index 69% rename from src/pycram/robot_descriptions/iCub_description.py rename to src/pycram/robot_descriptions/icub_description.py index 357f26549..4a76f82a7 100644 --- a/src/pycram/robot_descriptions/iCub_description.py +++ b/src/pycram/robot_descriptions/icub_description.py @@ -7,20 +7,18 @@ from ..helper import get_robot_mjcf_path import icub_models -#filename = get_ros_package_path('pycram') + '/resources/robots/' + "iCubGazeboV2_5_visuomanip" + '.urdf' -filename_ros_package = "/usr/local/src/robot/Bremen/workspace/ros/src/icub_model/urdf/model.urdf" +filename = get_ros_package_path('icub_model') + '/urdf/model' + '.urdf' -RobotName = 'iCubGazeboV2_5_visuomanip' -iCub_description = RobotDescription("iCub", +icub_description = RobotDescription("icub", "root_link", "torso_1", "torso_pitch", - filename_ros_package) + filename) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left", "chest", "l_hand", - iCub_description.urdf_object, arm_type=Arms.LEFT) + icub_description.urdf_object, arm_type=Arms.LEFT) left_arm.add_static_joint_states("park", {'l_shoulder_pitch': -80.0, 'l_shoulder_roll': 70.0, 'l_shoulder_yaw': 30.0, @@ -29,13 +27,13 @@ 'l_wrist_pitch': 0.0, 'l_wrist_yaw': 0.0}) -iCub_description.add_kinematic_chain_description(left_arm) +icub_description.add_kinematic_chain_description(left_arm) ################################## Left Gripper ################################## left_gripper = EndEffectorDescription("left_gripper", "l_hand", "l_hand", - iCub_description.urdf_object) + icub_description.urdf_object) left_arm.end_effector = left_gripper @@ -43,7 +41,7 @@ ################################## Right Arm ################################## right_arm = KinematicChainDescription("right", "chest", "r_hand", - iCub_description.urdf_object, arm_type=Arms.RIGHT) + icub_description.urdf_object, arm_type=Arms.RIGHT) right_arm.add_static_joint_states("park", {'r_shoulder_pitch': 80.0, 'r_shoulder_roll': 70.0, 'r_shoulder_yaw': 30.0, @@ -52,12 +50,12 @@ 'r_wrist_pitch': 0.0, 'r_wrist_yaw': 0.0}) -iCub_description.add_kinematic_chain_description(right_arm) +icub_description.add_kinematic_chain_description(right_arm) ################################## Right Gripper ################################## right_gripper = EndEffectorDescription("right_gripper", "r_hand", "r_hand", - iCub_description.urdf_object) + icub_description.urdf_object) right_arm.end_effector = right_gripper @@ -67,17 +65,17 @@ ################################## Camera ################################## camera = CameraDescription("left_camera", "eyes_tilt_frame", 1.27, 1.60, 0.99483, 0.75049) -iCub_description.add_camera_description(camera) +icub_description.add_camera_description(camera) ################################## Neck ################################## -iCub_description.add_kinematic_chain("neck", "neck_1", "head") +icub_description.add_kinematic_chain("neck", "neck_1", "head") ################################# Grasps ################################## -iCub_description.add_grasp_orientations({Grasp.FRONT: [0, 0, 0, 1], - Grasp.LEFT: [0, 0, -1, 1], - Grasp.RIGHT: [0, 0, 1, 1], - Grasp.TOP: [0, 1, 0, 1]}) +icub_description.add_grasp_orientations({Grasp.FRONT: [0, 0, 0, 1], + Grasp.LEFT: [0, 0, -1, 1], + Grasp.RIGHT: [0, 0, 1, 1], + Grasp.TOP: [0, 1, 0, 1]}) # Add to RobotDescriptionManager rdm = RobotDescriptionManager() -rdm.register_description(iCub_description) +rdm.register_description(icub_description) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index dc7840855..3867f8d78 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -951,7 +951,11 @@ def joint_states(self, joint_states: Dict[int, JointState]) -> None: joint.current_state = joint_states[joint.id] def robot_virtual_move_base_joints_names(self): - return self.robot_description.virtual_mobile_base_joints.names + try: + return self.robot_description.virtual_mobile_base_joints.names + except AttributeError: + # Means that robot has no virtual move base joints. + return [] def remove_saved_states(self) -> None: """ diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py new file mode 100644 index 000000000..e69de29bb diff --git a/test/testRandom.py b/test/testRandom.py index 2cf9378bb..208647c0d 100644 --- a/test/testRandom.py +++ b/test/testRandom.py @@ -1,9 +1,12 @@ +from bullet_world_testcase import BulletWorldTestCase from pycram.datastructures.pose import Pose from pycram.description import ObjectDescription from pycram.designators.motion_designator import LookingMotion, MoveTCPMotion -from pycram.process_module import ProcessModuleManager, simulated_robot +from pycram.ontology.ontology import OntologyManager, SOMA_ONTOLOGY_IRI +from pycram.process_module import ProcessModuleManager, simulated_robot, ProcessModule from pycram.robot_description import RobotDescription, RobotDescriptionManager -from pycram.datastructures.enums import GripperState, Arms, WorldMode +from pycram.datastructures.enums import GripperState, Arms, WorldMode, ExecutionType +from pycram.ros.ros_tools import sleep from pycram.worlds.bullet_world import BulletWorld from pycram.world_concepts.world_object import Object from pycram.datastructures.enums import ObjectType, WorldMode @@ -22,7 +25,7 @@ def run_look_and_move_tcp(): rm = RobotDescriptionManager() - rm.load_description('iCub') + rm.load_description('icub') with simulated_robot: pm = ProcessModuleManager.get_manager() @@ -34,17 +37,30 @@ def run_look_and_move_tcp(): pm.exit() -def try_world(): - extension: str = ObjectDescription.get_file_extension() - rdm = RobotDescriptionManager() - rdm.load_description("pr2") - world = BulletWorld(mode=WorldMode.DIRECT) - milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) +class ICUBTestCase(BulletWorldTestCase): - cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", - pose=Pose([1.3, 0.7, 0.95])) + @classmethod + def setUpClass(cls): + rm = RobotDescriptionManager() + rm.load_description('icub') + cls.world = BulletWorld(mode=WorldMode.GUI) + cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) + cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, + RobotDescription.current_robot_description.name + cls.extension, + pose=Pose([0, 0, 0.5])) + cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) + cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", + pose=Pose([1.3, 0.7, 0.95])) + ProcessModule.execution_delay = False + ProcessModuleManager.execution_type = ExecutionType.SIMULATED + cls.viz_marker_publisher = VizMarkerPublisher() + OntologyManager(SOMA_ONTOLOGY_IRI) + def test_try_world(self): + sleep(100) -run_look_and_move_tcp() + + +#run_look_and_move_tcp() From 3a0f739909a58db0ea3a7983d5d892ed992ecfdb Mon Sep 17 00:00:00 2001 From: oeldardear Date: Wed, 4 Dec 2024 15:03:19 +0100 Subject: [PATCH 06/22] [iCub] (WIP) added update part to robot description. added test move joints. --- .../process_modules/icub_process_modules.py | 92 +++++++++++-------- test/testRandom.py | 5 + 2 files changed, 61 insertions(+), 36 deletions(-) diff --git a/src/pycram/process_modules/icub_process_modules.py b/src/pycram/process_modules/icub_process_modules.py index 8c2868c42..9fbbe72a4 100644 --- a/src/pycram/process_modules/icub_process_modules.py +++ b/src/pycram/process_modules/icub_process_modules.py @@ -38,7 +38,7 @@ "r_hand_finger", "r_thumb_oppose","r_thumb_proximal","r_thumb_distal", "r_index_proximal","r_index_distal", - "l_middle_proximal","l_middle_distal", + "r_middle_proximal","r_middle_distal", "r_pinky"], "left_arm": ["l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", "l_elbow", @@ -78,6 +78,45 @@ def open_buffered_bottle_port(port_name): print(f"Port %s opened correctly" % port_name) return True , opened_port + +def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): + if len(joint_to_change_idx): + part_state: yarp.Bottle = state_port.read(shouldWait=True) + part_new_states = [] + for i in range(part_state.size()): + print(i, " ", part_state.get(i).asFloat32()) + part_new_states.append(part_state.get(i).asFloat32()) + + for i in range(len(joint_to_change_idx)): + part_new_states[joint_to_change_idx[i]] = joints_to_change_pos[i] + + yarp_bottle_msg: yarp.Bottle = yarp.Bottle() + yarp_bottle_reply: yarp.Bottle = yarp.Bottle() + yarp_bottle_msg.addVocab32('c', 't', 'p', 'q') + yarp_bottle_msg.addVocab32('t', 'i', 'm', 'e') + yarp_bottle_msg.addFloat32(1.5) + yarp_bottle_msg.addVocab32('o', 'f', 'f') + yarp_bottle_msg.addInt32(0) + yarp_bottle_msg.addVocab32('p', 'o', 's') + target_loc: yarp.Bottle = yarp_bottle_msg.addList() + for i in part_new_states: + target_loc.addFloat32(i) + + print(f"command Ready to send to iCub torso tcp") + + ctp_port.write(yarp_bottle_msg, yarp_bottle_reply) + reply_vocab = yarp_bottle_reply.get(0).asVocab32() + + if reply_vocab == NO_ACK_VOCAB: + print("NO_ACK") + return False + elif reply_vocab == ACK_VOCAB: + print("ACK") + return True + else: + print("another reply") + return False + class iCubNavigation(ProcessModule): """ The process module to move the robot from one position to another. @@ -234,6 +273,10 @@ def get_joint_indices(self,joint_name): Returns: tuple: (part_index, joint_index) or (None, None) if joint not found. """ + left_arm_chain = RobotDescription.current_robot_description.get_arm_chain(Arms.LEFT) + print(left_arm_chain.joint_names) + right_arm_chain = RobotDescription.current_robot_description.get_arm_chain(Arms.RIGHT) + print(right_arm_chain.joint_names) for part_index, (part_name, joints) in enumerate(robot_parts_list): if joint_name in joints: joint_index = joints.index(joint_name) @@ -271,43 +314,20 @@ def _execute(self, desig: MoveJointsMotion): index += 1 - if len(torso_to_change_joints): - torso_state: yarp.Bottle = self.state_ports[0].read(shouldWait=True) - torso_new_states = [] - for i in range(torso_state.size()): - print(i," ",torso_state.get(i).asFloat32()) - torso_new_states.append(torso_state.get(i).asFloat32()) - - for i in range(len(torso_to_change_joints)): - torso_new_states[torso_to_change_joints[i]] = torso_to_change_joints_states[i] - - yarp_bottle_msg: yarp.Bottle = yarp.Bottle() - yarp_bottle_reply: yarp.Bottle = yarp.Bottle() - yarp_bottle_msg.addVocab32('c', 't', 'p', 'q') - yarp_bottle_msg.addVocab32('t', 'i', 'm', 'e') - yarp_bottle_msg.addFloat32(1.5) - yarp_bottle_msg.addVocab32('o', 'f', 'f') - yarp_bottle_msg.addInt32(0) - yarp_bottle_msg.addVocab32('p', 'o', 's') - target_loc: yarp.Bottle = yarp_bottle_msg.addList() - for i in torso_new_states: - target_loc.addFloat32(i) - - print(f"command Ready to send to iCub torso tcp") + update_part(self.state_ports[0], + self.ctp_ports[0], + torso_to_change_joints, + torso_to_change_joints_states) - self.ctp_ports[0].write(yarp_bottle_msg, yarp_bottle_reply) - reply_vocab = yarp_bottle_reply.get(0).asVocab32() - - if reply_vocab == NO_ACK_VOCAB: - print("NO_ACK") - return False - elif reply_vocab == ACK_VOCAB: - print("ACK") - return True - else: - print("another reply") - return False + update_part(self.state_ports[1], + self.ctp_ports[1], + right_arm_to_change_joints, + right_arm_to_change_joints_states) + update_part(self.state_ports[2], + self.ctp_ports[2], + left_arm_to_change_joints, + left_arm_to_change_joints_states) diff --git a/test/testRandom.py b/test/testRandom.py index 208647c0d..09f8ee90e 100644 --- a/test/testRandom.py +++ b/test/testRandom.py @@ -56,6 +56,11 @@ def setUpClass(cls): cls.viz_marker_publisher = VizMarkerPublisher() OntologyManager(SOMA_ONTOLOGY_IRI) + + def test_move_joints(self): + with simulated_robot: + MoveJointsMotion(["torso_roll"], [20.0]).perform() + def test_try_world(self): sleep(100) From 0a788fc6891571c45936d3e3735cdfaf1ba08604 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Wed, 4 Dec 2024 15:41:24 +0100 Subject: [PATCH 07/22] [iCub] updating robot description to include actuated joints --- .../process_modules/icub_process_modules.py | 44 +--------------- src/pycram/robot_description.py | 31 ++++++++++-- .../robot_descriptions/icub_description.py | 23 ++++++++- test/bullet_world_testcase.py | 0 test/testRandom.py | 50 ++++++------------- 5 files changed, 66 insertions(+), 82 deletions(-) delete mode 100644 test/bullet_world_testcase.py diff --git a/src/pycram/process_modules/icub_process_modules.py b/src/pycram/process_modules/icub_process_modules.py index 9fbbe72a4..cbeb50b9e 100644 --- a/src/pycram/process_modules/icub_process_modules.py +++ b/src/pycram/process_modules/icub_process_modules.py @@ -30,29 +30,6 @@ ACK_VOCAB = yarp.createVocab32('a','c','k') NO_ACK_VOCAB = yarp.createVocab32('n','a','c','k') -robot_parts = { - "torso": ["torso_yaw", "torso_roll", "torso_pitch"], - "right_arm": ["r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", - "r_elbow", - "r_wrist_prosup","r_wrist_pitch","r_wrist_yaw", - "r_hand_finger", - "r_thumb_oppose","r_thumb_proximal","r_thumb_distal", - "r_index_proximal","r_index_distal", - "r_middle_proximal","r_middle_distal", - "r_pinky"], - "left_arm": ["l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", - "l_elbow", - "l_wrist_prosup","l_wrist_pitch","l_wrist_yaw", - "l_hand_finger", - "l_thumb_oppose","l_thumb_proximal","l_thumb_distal", - "l_index_proximal","l_index_distal", - "l_middle_proximal","l_middle_distal", - "l_pinky"], - } - -# Convert to a list of parts for indexed access -robot_parts_list = list(robot_parts.items()) - def init_yarp_network(): if not yarp.Network.checkNetwork(): @@ -263,25 +240,6 @@ def __init__(self, lock, self.state_ports = state_ports self.ctp_ports = ctp_ports - def get_joint_indices(self,joint_name): - """ - Given a joint name, returns the indices of the part and the joint within that part. - - Args: - joint_name (str): Name of the joint to find. - - Returns: - tuple: (part_index, joint_index) or (None, None) if joint not found. - """ - left_arm_chain = RobotDescription.current_robot_description.get_arm_chain(Arms.LEFT) - print(left_arm_chain.joint_names) - right_arm_chain = RobotDescription.current_robot_description.get_arm_chain(Arms.RIGHT) - print(right_arm_chain.joint_names) - for part_index, (part_name, joints) in enumerate(robot_parts_list): - if joint_name in joints: - joint_index = joints.index(joint_name) - return part_index, joint_index - return None, None def _execute(self, desig: MoveJointsMotion): @@ -298,7 +256,7 @@ def _execute(self, desig: MoveJointsMotion): index = 0 for joint_mame in to_change_joints: - part_index,joint_index = self.get_joint_indices(joint_mame) + part_index,joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices(joint_mame) if part_index is not None: if part_index == 0: torso_to_change_joints.append(joint_index) diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 495267a3a..17634a4a9 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -1,6 +1,6 @@ # used for delayed evaluation of typing until python 3.11 becomes mainstream from __future__ import annotations -from typing_extensions import List, Dict, Union, Optional +from typing_extensions import List, Dict, Union, Optional, Tuple from .datastructures.dataclasses import VirtualMobileBaseJoints from .datastructures.enums import Arms, Grasp, GripperState, GripperType, JointType @@ -118,7 +118,8 @@ class RobotDescription: """ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, urdf_path: str, - virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None): + virtual_mobile_base_joints: Optional[VirtualMobileBaseJoints] = None, mjcf_path: Optional[str] = None, + actuated_joints: Optional[Dict] = None): """ Initialize the RobotDescription. The URDF is loaded from the given path and used as basis for the kinematic chains. @@ -130,6 +131,7 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, :param urdf_path: Path to the URDF file of the robot :param virtual_mobile_base_joints: Virtual mobile base joint names for mobile robots :param mjcf_path: Path to the MJCF file of the robot + :param actuated_joints: Actuated joints of the robot. """ self.name = name self.base_link = base_link @@ -139,7 +141,11 @@ def __init__(self, name: str, base_link: str, torso_link: str, torso_joint: str, # Since parsing URDF causes a lot of warning messages which can't be deactivated, we suppress them self.urdf_object = URDFObject(urdf_path) self.joint_types = {joint.name: joint.type for joint in self.urdf_object.joints} - self.joint_actuators: Optional[Dict] = parse_mjcf_actuators(mjcf_path) if mjcf_path is not None else None + self.joint_actuators: Optional[Dict] = None + if actuated_joints is not None: + self.joint_actuators = actuated_joints + elif mjcf_path is not None: + self.joint_actuators = parse_mjcf_actuators(mjcf_path) self.kinematic_chains: Dict[str, KinematicChainDescription] = {} self.cameras: Dict[str, CameraDescription] = {} self.grasps: Dict[Grasp, List[float]] = {} @@ -156,7 +162,7 @@ def has_actuators(self): """ return self.joint_actuators is not None - def get_actuator_for_joint(self, joint: str) -> Optional[str]: + def get_actuator_for_joint(self, joint: str) -> Optional[Union[str, List[str]]]: """ Get the actuator name for a given joint. @@ -167,6 +173,23 @@ def get_actuator_for_joint(self, joint: str) -> Optional[str]: return self.joint_actuators.get(joint) return None + def get_actuated_joint_indices(self, joint_name) -> Tuple[Optional[int], Optional[int]]: + """ + Given a joint name, returns the indices of the part and the joint within that part. + + Args: + joint_name (str): Name of the joint to find. + + Returns: + tuple: (part_index, joint_index) or (None, None) if joint not found. + """ + + for part_index, (part_name, joints) in enumerate(list(self.joint_actuators.items())): + if joint_name in joints: + joint_index = joints.index(joint_name) + return part_index, joint_index + return None, None + def add_kinematic_chain_description(self, chain: KinematicChainDescription): """ Adds a KinematicChainDescription object to the RobotDescription. The chain is stored with the name of the chain diff --git a/src/pycram/robot_descriptions/icub_description.py b/src/pycram/robot_descriptions/icub_description.py index 4a76f82a7..457465132 100644 --- a/src/pycram/robot_descriptions/icub_description.py +++ b/src/pycram/robot_descriptions/icub_description.py @@ -9,12 +9,33 @@ filename = get_ros_package_path('icub_model') + '/urdf/model' + '.urdf' +actuated_joints = { + "torso": ["torso_yaw", "torso_roll", "torso_pitch"], + "right_arm": ["r_shoulder_pitch", "r_shoulder_roll", "r_shoulder_yaw", + "r_elbow", + "r_wrist_prosup","r_wrist_pitch","r_wrist_yaw", + "r_hand_finger", + "r_thumb_oppose","r_thumb_proximal","r_thumb_distal", + "r_index_proximal","r_index_distal", + "r_middle_proximal","r_middle_distal", + "r_pinky"], + "left_arm": ["l_shoulder_pitch", "l_shoulder_roll", "l_shoulder_yaw", + "l_elbow", + "l_wrist_prosup","l_wrist_pitch","l_wrist_yaw", + "l_hand_finger", + "l_thumb_oppose","l_thumb_proximal","l_thumb_distal", + "l_index_proximal","l_index_distal", + "l_middle_proximal","l_middle_distal", + "l_pinky"], + } + icub_description = RobotDescription("icub", "root_link", "torso_1", "torso_pitch", - filename) + filename, + actuated_joints=actuated_joints) ################################## Left Arm ################################## left_arm = KinematicChainDescription("left", "chest", "l_hand", diff --git a/test/bullet_world_testcase.py b/test/bullet_world_testcase.py deleted file mode 100644 index e69de29bb..000000000 diff --git a/test/testRandom.py b/test/testRandom.py index 09f8ee90e..4dac1c50d 100644 --- a/test/testRandom.py +++ b/test/testRandom.py @@ -1,27 +1,18 @@ -from bullet_world_testcase import BulletWorldTestCase -from pycram.datastructures.pose import Pose -from pycram.description import ObjectDescription -from pycram.designators.motion_designator import LookingMotion, MoveTCPMotion -from pycram.ontology.ontology import OntologyManager, SOMA_ONTOLOGY_IRI -from pycram.process_module import ProcessModuleManager, simulated_robot, ProcessModule -from pycram.robot_description import RobotDescription, RobotDescriptionManager -from pycram.datastructures.enums import GripperState, Arms, WorldMode, ExecutionType +import pycrap +from pycram.testing import BulletWorldTestCase +from pycram.process_module import ProcessModuleManager, ProcessModule +from pycram.robot_description import RobotDescriptionManager +from pycram.datastructures.enums import ExecutionType from pycram.ros.ros_tools import sleep -from pycram.worlds.bullet_world import BulletWorld -from pycram.world_concepts.world_object import Object -from pycram.datastructures.enums import ObjectType, WorldMode from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher from pycram.worlds.bullet_world import BulletWorld from pycram.designators.action_designator import * from pycram.designators.location_designator import * -from pycram.designators.object_designator import * -from pycram.datastructures.enums import ObjectType, WorldMode +from pycram.datastructures.enums import WorldMode from pycram.datastructures.pose import Pose -from pycram.process_module import simulated_robot, with_simulated_robot -from pycram.object_descriptors.urdf import ObjectDescription +from pycram.process_module import simulated_robot from pycram.world_concepts.world_object import Object -from pycram.datastructures.dataclasses import Color -from pycram.designators.object_designator import BelieveObject + def run_look_and_move_tcp(): rm = RobotDescriptionManager() @@ -33,39 +24,30 @@ def run_look_and_move_tcp(): LookingMotion(Pose([-2, -2, 3])).perform() time.sleep(2) MoveTCPMotion(Pose([-0.2, 0.1, 0.1]), Arms.LEFT).perform() - MoveJointsMotion(["torso_roll"],[20.0]).perform() + MoveJointsMotion(["torso_roll"], [20.0]).perform() pm.exit() + class ICUBTestCase(BulletWorldTestCase): @classmethod def setUpClass(cls): - rm = RobotDescriptionManager() - rm.load_description('icub') cls.world = BulletWorld(mode=WorldMode.GUI) - cls.milk = Object("milk", ObjectType.MILK, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cls.robot = Object(RobotDescription.current_robot_description.name, ObjectType.ROBOT, - RobotDescription.current_robot_description.name + cls.extension, - pose=Pose([0, 0, 0.5])) - cls.kitchen = Object("kitchen", ObjectType.ENVIRONMENT, "kitchen" + cls.extension) - cls.cereal = Object("cereal", ObjectType.BREAKFAST_CEREAL, "breakfast_cereal.stl", + cls.milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) + cls.robot = Object("icub", pycrap.Robot, "icub" + cls.extension, pose=Pose([0, 0, 0.5])) + cls.kitchen = Object("kitchen", pycrap.Kitchen, "kitchen" + cls.extension) + cls.cereal = Object("cereal", pycrap.Cereal, "breakfast_cereal.stl", pose=Pose([1.3, 0.7, 0.95])) ProcessModule.execution_delay = False ProcessModuleManager.execution_type = ExecutionType.SIMULATED cls.viz_marker_publisher = VizMarkerPublisher() - OntologyManager(SOMA_ONTOLOGY_IRI) - def test_move_joints(self): with simulated_robot: - MoveJointsMotion(["torso_roll"], [20.0]).perform() + MoveJointsMotion(["torso_roll"], [-20.0]).perform() def test_try_world(self): sleep(100) - - - - -#run_look_and_move_tcp() +# run_look_and_move_tcp() From 316e7a18e88e95883747ca43b6e2d5433a778c1c Mon Sep 17 00:00:00 2001 From: oeldardear Date: Wed, 4 Dec 2024 16:56:45 +0100 Subject: [PATCH 08/22] [iCub] updating robot description to include fingers and left camera --- .../process_modules/icub_process_modules.py | 61 ++++++++++++++++++- .../robot_descriptions/icub_description.py | 35 +++++++++-- test/testRandom.py | 4 +- 3 files changed, 93 insertions(+), 7 deletions(-) diff --git a/src/pycram/process_modules/icub_process_modules.py b/src/pycram/process_modules/icub_process_modules.py index cbeb50b9e..bf024e879 100644 --- a/src/pycram/process_modules/icub_process_modules.py +++ b/src/pycram/process_modules/icub_process_modules.py @@ -225,7 +225,63 @@ class iCubMoveArmJoints(ProcessModule): list that should be applied or a pre-defined position can be used, such as "parking" """ + def __init__(self, lock, + state_ports : [yarp.BufferedPortBottle], + ctp_ports: [yarp.RpcClient]): + super().__init__(lock) + self.state_ports = state_ports + self.ctp_ports = ctp_ports + print("iCubMoveArmJoints initialized") + + def _execute(self, desig: MoveArmJointsMotion): + + right_arm_to_change_joints = [] + left_arm_to_change_joints = [] + + right_arm_to_change_joints_states = [] + left_arm_to_change_joints_states = [] + + right_arm_to_change = desig.right_arm_poses + left_arm_to_change = desig.left_arm_poses + + if right_arm_to_change is not None: + for joint_mame , joint_pose in right_arm_to_change.items(): + part_index,joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices(joint_mame) + if part_index is not None: + if part_index == 1: + right_arm_to_change_joints.append(joint_index) + right_arm_to_change_joints_states.append(joint_pose) + else: + print("error in joint name") + else: + print("error in joint name") + if left_arm_to_change is not None: + for joint_mame, joint_pose in left_arm_to_change.items(): + part_index, joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices( + joint_mame) + if part_index is not None: + if part_index == 2: + left_arm_to_change_joints.append(joint_index) + left_arm_to_change_joints_states.append(joint_pose) + else: + print("error in joint name") + else: + print("error in joint name") + + + update_part(self.state_ports[0], + self.ctp_ports[0], + right_arm_to_change_joints, + right_arm_to_change_joints_states) + + update_part(self.state_ports[1], + self.ctp_ports[1], + left_arm_to_change_joints, + left_arm_to_change_joints_states) + + + print("iCub Move Arm Joints") class iCubMoveJoints(ProcessModule): @@ -479,7 +535,10 @@ def move_tcp(self): def move_arm_joints(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: - return iCubMoveArmJoints(self._move_arm_joints_lock) + return iCubMoveArmJoints(self._move_arm_joints_lock, + [self.state_right_arm_port, self.state_right_arm_port], + [self.ctp_right_arm_client_port,self.ctp_right_arm_client_port]) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: return iCubMoveArmJointsReal(self._move_arm_joints_lock) diff --git a/src/pycram/robot_descriptions/icub_description.py b/src/pycram/robot_descriptions/icub_description.py index 457465132..c2422bc74 100644 --- a/src/pycram/robot_descriptions/icub_description.py +++ b/src/pycram/robot_descriptions/icub_description.py @@ -38,7 +38,7 @@ actuated_joints=actuated_joints) ################################## Left Arm ################################## -left_arm = KinematicChainDescription("left", "chest", "l_hand", +left_arm = KinematicChainDescription("left_arm", "chest", "l_hand", icub_description.urdf_object, arm_type=Arms.LEFT) left_arm.add_static_joint_states("park", {'l_shoulder_pitch': -80.0, 'l_shoulder_roll': 70.0, @@ -61,7 +61,7 @@ ################################## Right Arm ################################## -right_arm = KinematicChainDescription("right", "chest", "r_hand", +right_arm = KinematicChainDescription("right_arm", "chest", "r_hand", icub_description.urdf_object, arm_type=Arms.RIGHT) right_arm.add_static_joint_states("park", {'r_shoulder_pitch': 80.0, 'r_shoulder_roll': 70.0, @@ -84,12 +84,37 @@ ################################## Camera ################################## -camera = CameraDescription("left_camera", "eyes_tilt_frame", 1.27, +left_camera = CameraDescription("left_camera", "l_eye", 1.27, 1.60, 0.99483, 0.75049) -icub_description.add_camera_description(camera) +icub_description.add_camera_description(left_camera) + +right_camera = CameraDescription("right_camera", "r_eye", 1.27, + 1.60, 0.99483, 0.75049) +icub_description.add_camera_description(right_camera) ################################## Neck ################################## -icub_description.add_kinematic_chain("neck", "neck_1", "head") +icub_description.add_kinematic_chain("neck", "chest", "head") + + +################################## l_fingers ################################## +icub_description.add_kinematic_chain("l_index", "l_hand", "l_hand_index_tip") +icub_description.add_kinematic_chain("l_little", "l_hand", "l_hand_little_tip") +icub_description.add_kinematic_chain("l_middle", "l_hand", "l_hand_middle_tip") +icub_description.add_kinematic_chain("l_ring", "l_hand", "l_hand_ring_tip") +icub_description.add_kinematic_chain("l_thumb", "l_hand", "l_hand_thumb_tip") + + +################################## r_ringers ################################## +icub_description.add_kinematic_chain("r_index", "r_hand", "r_hand_index_tip") +icub_description.add_kinematic_chain("r_little", "r_hand", "r_hand_little_tip") +icub_description.add_kinematic_chain("r_middle", "r_hand", "r_hand_middle_tip") +icub_description.add_kinematic_chain("r_ring", "r_hand", "r_hand_ring_tip") +icub_description.add_kinematic_chain("r_thumb", "r_hand", "r_hand_thumb_tip") + + +################################## legs ################################## +icub_description.add_kinematic_chain("l_leg", "root_link", "l_foot") +icub_description.add_kinematic_chain("r_leg", "root_link", "r_foot") ################################# Grasps ################################## icub_description.add_grasp_orientations({Grasp.FRONT: [0, 0, 0, 1], diff --git a/test/testRandom.py b/test/testRandom.py index 4dac1c50d..7777d15d7 100644 --- a/test/testRandom.py +++ b/test/testRandom.py @@ -46,8 +46,10 @@ def setUpClass(cls): def test_move_joints(self): with simulated_robot: MoveJointsMotion(["torso_roll"], [-20.0]).perform() + MoveArmJointsMotion(left_arm_poses={"r_elbow":20.0}).perform() + MoveArmJointsMotion(right_arm_poses={"r_elbow": 20.0}).perform() def test_try_world(self): - sleep(100) + sleep(10) # run_look_and_move_tcp() From b0a980e58ad0a27e80a81580577db9c050c250cd Mon Sep 17 00:00:00 2001 From: oeldardear Date: Thu, 5 Dec 2024 14:12:55 +0100 Subject: [PATCH 09/22] [iCub] radian angles - meshes scale - using default simulation methods - upating few tests --- launch/ik_and_description.launch | 2 +- .../process_modules/icub_process_modules.py | 142 +++++++----------- src/pycram/robot_description.py | 6 +- .../robot_descriptions/icub_description.py | 30 ++-- src/pycram/ros_utils/viz_marker_publisher.py | 5 +- test/testRandom.py | 48 ++++-- 6 files changed, 116 insertions(+), 117 deletions(-) diff --git a/launch/ik_and_description.launch b/launch/ik_and_description.launch index 6ffb31ed0..6d4b68126 100644 --- a/launch/ik_and_description.launch +++ b/launch/ik_and_description.launch @@ -18,7 +18,7 @@ - + diff --git a/src/pycram/process_modules/icub_process_modules.py b/src/pycram/process_modules/icub_process_modules.py index bf024e879..0a980a0d2 100644 --- a/src/pycram/process_modules/icub_process_modules.py +++ b/src/pycram/process_modules/icub_process_modules.py @@ -1,3 +1,4 @@ +import math from threading import Lock from typing import Union @@ -5,6 +6,8 @@ import actionlib +from .default_process_modules import DefaultMoveJoints, DefaultMoveArmJoints, DefaultMoveTCP, DefaultNavigation, \ + DefaultMoveHead, DefaultDetecting, DefaultMoveGripper from .. import world_reasoning as btr import numpy as np @@ -65,7 +68,7 @@ def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): part_new_states.append(part_state.get(i).asFloat32()) for i in range(len(joint_to_change_idx)): - part_new_states[joint_to_change_idx[i]] = joints_to_change_pos[i] + part_new_states[joint_to_change_idx[i]] = math.degrees(joints_to_change_pos[i]) yarp_bottle_msg: yarp.Bottle = yarp.Bottle() yarp_bottle_reply: yarp.Bottle = yarp.Bottle() @@ -79,7 +82,7 @@ def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): for i in part_new_states: target_loc.addFloat32(i) - print(f"command Ready to send to iCub torso tcp") + print("command Ready to send to iCub part tcp") ctp_port.write(yarp_bottle_msg, yarp_bottle_reply) reply_vocab = yarp_bottle_reply.get(0).asVocab32() @@ -94,7 +97,7 @@ def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): print("another reply") return False -class iCubNavigation(ProcessModule): +class iCubNavigationReal(ProcessModule): """ The process module to move the robot from one position to another. """ @@ -103,7 +106,7 @@ def _execute(self, desig: MoveMotion): print("iCubNavigate") -class iCubMoveHead(ProcessModule): +class iCubMoveHeadReal(ProcessModule): """ This process module moves the head to look at a specific point in the world coordinate frame. This point can either be a position or an object. @@ -148,7 +151,7 @@ def _execute(self, desig: LookingMotion): -class iCubMoveGripper(ProcessModule): +class iCubMoveGripperReal(ProcessModule): """ This process module controls the gripper of the robot. They can either be opened or closed. Furthermore, it can only moved one gripper at a time. @@ -158,7 +161,7 @@ def _execute(self, desig: MoveGripperMotion): print("iCub Move Gripper") -class iCubDetecting(ProcessModule): +class iCubDetectingReal(ProcessModule): """ This process module tries to detect an object with the given type. To be detected the object has to be in the field of view of the robot. @@ -167,7 +170,7 @@ class iCubDetecting(ProcessModule): def _execute(self, desig: DetectingMotion): print("iCub Detect") -class iCubMoveTCP(ProcessModule): +class iCubMoveTCPReal(ProcessModule): """ This process moves the tool center point of either the right or the left arm. """ @@ -219,7 +222,7 @@ def _execute(self, desig: MoveTCPMotion): -class iCubMoveArmJoints(ProcessModule): +class iCubMoveArmJointsReal(ProcessModule): """ This process modules moves the joints of either the right or the left arm. The joint states can be given as list that should be applied or a pre-defined position can be used, such as "parking" @@ -247,7 +250,7 @@ def _execute(self, desig: MoveArmJointsMotion): if right_arm_to_change is not None: for joint_mame , joint_pose in right_arm_to_change.items(): - part_index,joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices(joint_mame) + part_name,part_index,joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices(joint_mame) if part_index is not None: if part_index == 1: right_arm_to_change_joints.append(joint_index) @@ -258,7 +261,7 @@ def _execute(self, desig: MoveArmJointsMotion): print("error in joint name") if left_arm_to_change is not None: for joint_mame, joint_pose in left_arm_to_change.items(): - part_index, joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices( + part_name,part_index, joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices( joint_mame) if part_index is not None: if part_index == 2: @@ -284,7 +287,7 @@ def _execute(self, desig: MoveArmJointsMotion): print("iCub Move Arm Joints") -class iCubMoveJoints(ProcessModule): +class iCubMoveJointsReal(ProcessModule): """ Process Module for generic joint movements, is not confined to the arms but can move any joint of the robot """ @@ -312,7 +315,7 @@ def _execute(self, desig: MoveJointsMotion): index = 0 for joint_mame in to_change_joints: - part_index,joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices(joint_mame) + part_name,part_index,joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices(joint_mame) if part_index is not None: if part_index == 0: torso_to_change_joints.append(joint_index) @@ -354,104 +357,70 @@ class iCubWorldStateDetecting(ProcessModule): """ def _execute(self, desig: WorldStateDetectingMotion): - print("iCub World State Detecting") - - -class iCubOpen(ProcessModule): - """ - Low-level implementation of opening a container in the simulation. Assumes the handle is already grasped. - """ - - def _execute(self, desig: OpeningMotion): - print("iCub Open") + obj_type = desig.object_type + return list(filter(lambda obj: obj.obj_type == obj_type, World.current_world.objects))[0] -class iCubClose(ProcessModule): - """ - Low-level implementation that lets the robot close a grasped container, in simulation - """ - - def _execute(self, desig: ClosingMotion): - print("iCub Close") - ########################################################### -########## Process Modules for the Real iCub ############## +######## Process Modules for the simulated iCub ########### ########################################################### -class iCubNavigationReal(ProcessModule): +class iCubNavigation(DefaultNavigation): """ Process module for the real PR2 that sends a cartesian goal to giskard to move the robot base """ + ... - def _execute(self, designator: MoveMotion) -> Any: - print("iCub Navigate Real") +class iCubMoveHead(DefaultMoveHead): + """ + This process module moves the head to look at a specific point in the world coordinate frame. + This point can either be a position or an object. + """ -class iCubDetectingReal(ProcessModule): + ... + + +class iCubDetecting(DefaultDetecting): """ Process Module for the real Pr2 that tries to detect an object fitting the given object description. Uses Robokudo for perception of the environment. """ - def _execute(self, designator: DetectingMotion) -> Any: - print("iCub Detecting Real") + ... -class iCubMoveTCPReal(ProcessModule): +class iCubMoveTCP(DefaultMoveTCP): """ Moves the tool center point of the real PR2 while avoiding all collisions """ - def _execute(self, designator: MoveTCPMotion) -> Any: - print("iCub Move TCP Real") + ... -class iCubMoveArmJointsReal(ProcessModule): +class iCubMoveArmJoints(DefaultMoveArmJoints): """ Moves the arm joints of the real iCub to the given configuration while avoiding all collisions """ + ... - def _execute(self, designator: MoveArmJointsMotion) -> Any: - print("iCub Move Arm Joints Real") - -class iCubMoveJointsReal(ProcessModule): +class iCubMoveJoints(DefaultMoveJoints): """ Moves any joint using giskard, avoids all collisions while doint this. """ - - def _execute(self, designator: MoveJointsMotion) -> Any: - print("iCub Move Joints Real") + ... -class iCubMoveGripperReal(ProcessModule): +class iCubMoveGripper(DefaultMoveGripper): """ Opens or closes the gripper of the real PR2, gripper uses an action server for this instead of giskard """ - def _execute(self, designator: MoveGripperMotion) -> Any: - print("iCub Move Gripper Real") - - -class iCubOpenReal(ProcessModule): - """ - Tries to open an already grasped container - """ - - def _execute(self, designator: OpeningMotion) -> Any: - print("iCub Open Real") - - -class iCubCloseReal(ProcessModule): - """ - Tries to close an already grasped container - """ - - def _execute(self, designator: ClosingMotion) -> Any: - print("iCub Close Real") + ... class ICubManager(ProcessModuleManager): @@ -517,9 +486,9 @@ def navigate(self): def looking(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: - return iCubMoveHead(self._looking_lock,self.gaze_client_port) + return iCubMoveHead(self._looking_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: - return iCubMoveHead(self._looking_lock,self.gaze_client_port) + return iCubMoveHeadReal(self._looking_lock,self.gaze_client_port) def detecting(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: @@ -529,18 +498,18 @@ def detecting(self): def move_tcp(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: - return iCubMoveTCP(self._move_tcp_lock,self.action_client_port) + return iCubMoveTCP(self._move_tcp_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: - return iCubMoveTCPReal(self._move_tcp_lock) + return iCubMoveTCPReal(self._move_tcp_lock,self.action_client_port) def move_arm_joints(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: - return iCubMoveArmJoints(self._move_arm_joints_lock, - [self.state_right_arm_port, self.state_right_arm_port], - [self.ctp_right_arm_client_port,self.ctp_right_arm_client_port]) + return iCubMoveArmJoints(self._move_arm_joints_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: - return iCubMoveArmJointsReal(self._move_arm_joints_lock) + return iCubMoveArmJointsReal(self._move_arm_joints_lock, + [self.state_right_arm_port, self.state_left_arm_port], + [self.ctp_right_arm_client_port,self.ctp_left_arm_client_port]) def world_state_detecting(self): if (ProcessModuleManager.execution_type == ExecutionType.SIMULATED or @@ -549,11 +518,12 @@ def world_state_detecting(self): def move_joints(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: - return iCubMoveJoints(self._move_joints_lock, - [self.state_torso_port,self.state_right_arm_port,self.state_right_arm_port], - [self.ctp_torso_client_port,self.ctp_right_arm_client_port,self.ctp_right_arm_client_port]) + return iCubMoveJoints(self._move_joints_lock) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: - return iCubMoveJointsReal(self._move_joints_lock) + return iCubMoveJointsReal(self._move_joints_lock, + [self.state_torso_port,self.state_right_arm_port,self.state_left_arm_port], + [self.ctp_torso_client_port,self.ctp_right_arm_client_port,self.ctp_left_arm_client_port]) def move_gripper(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: @@ -562,16 +532,10 @@ def move_gripper(self): return iCubMoveGripperReal(self._move_gripper_lock) def open(self): - if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: - return iCubOpen(self._open_lock) - elif ProcessModuleManager.execution_type == ExecutionType.REAL: - return iCubOpenReal(self._open_lock) + print("iCub doesn't perform open action from here") def close(self): - if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: - return iCubClose(self._close_lock) - elif ProcessModuleManager.execution_type == ExecutionType.REAL: - return iCubCloseReal(self._close_lock) + print("iCub doesn't perform close action from here") def config_yarp_ports(self)->bool: diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index 17634a4a9..ece78951d 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -173,7 +173,7 @@ def get_actuator_for_joint(self, joint: str) -> Optional[Union[str, List[str]]]: return self.joint_actuators.get(joint) return None - def get_actuated_joint_indices(self, joint_name) -> Tuple[Optional[int], Optional[int]]: + def get_actuated_joint_indices(self, joint_name) -> Tuple[Optional[str],Optional[int], Optional[int]]: """ Given a joint name, returns the indices of the part and the joint within that part. @@ -187,8 +187,8 @@ def get_actuated_joint_indices(self, joint_name) -> Tuple[Optional[int], Optiona for part_index, (part_name, joints) in enumerate(list(self.joint_actuators.items())): if joint_name in joints: joint_index = joints.index(joint_name) - return part_index, joint_index - return None, None + return part_name,part_index, joint_index + return None,None, None def add_kinematic_chain_description(self, chain: KinematicChainDescription): """ diff --git a/src/pycram/robot_descriptions/icub_description.py b/src/pycram/robot_descriptions/icub_description.py index c2422bc74..d6b24192f 100644 --- a/src/pycram/robot_descriptions/icub_description.py +++ b/src/pycram/robot_descriptions/icub_description.py @@ -1,3 +1,5 @@ +import math + from ..datastructures.dataclasses import VirtualMobileBaseJoints from ..robot_description import RobotDescription, KinematicChainDescription, EndEffectorDescription, \ RobotDescriptionManager, CameraDescription @@ -40,13 +42,13 @@ ################################## Left Arm ################################## left_arm = KinematicChainDescription("left_arm", "chest", "l_hand", icub_description.urdf_object, arm_type=Arms.LEFT) -left_arm.add_static_joint_states("park", {'l_shoulder_pitch': -80.0, - 'l_shoulder_roll': 70.0, - 'l_shoulder_yaw': 30.0, - 'l_elbow': 70.0, - 'l_wrist_prosup': -25.0, - 'l_wrist_pitch': 0.0, - 'l_wrist_yaw': 0.0}) +left_arm.add_static_joint_states("park", {'l_shoulder_pitch': math.radians(-80.0), + 'l_shoulder_roll': math.radians(70.0), + 'l_shoulder_yaw': math.radians(30.0), + 'l_elbow': math.radians(70.0), + 'l_wrist_prosup': math.radians(-25.0), + 'l_wrist_pitch': math.radians(0.0), + 'l_wrist_yaw': math.radians(0.0)}) icub_description.add_kinematic_chain_description(left_arm) @@ -63,13 +65,13 @@ ################################## Right Arm ################################## right_arm = KinematicChainDescription("right_arm", "chest", "r_hand", icub_description.urdf_object, arm_type=Arms.RIGHT) -right_arm.add_static_joint_states("park", {'r_shoulder_pitch': 80.0, - 'r_shoulder_roll': 70.0, - 'r_shoulder_yaw': 30.0, - 'r_elbow': 70.0, - 'r_wrist_prosup': -25.0, - 'r_wrist_pitch': 0.0, - 'r_wrist_yaw': 0.0}) +right_arm.add_static_joint_states("park", {'r_shoulder_pitch': math.radians(-80.0), + 'r_shoulder_roll': math.radians(70.0), + 'r_shoulder_yaw': math.radians(30.0), + 'r_elbow': math.radians(70.0), + 'r_wrist_prosup': math.radians(-25.0), + 'r_wrist_pitch': math.radians(0.0), + 'r_wrist_yaw': math.radians(0.0)}) icub_description.add_kinematic_chain_description(right_arm) diff --git a/src/pycram/ros_utils/viz_marker_publisher.py b/src/pycram/ros_utils/viz_marker_publisher.py index 3454759b8..739b66eb3 100644 --- a/src/pycram/ros_utils/viz_marker_publisher.py +++ b/src/pycram/ros_utils/viz_marker_publisher.py @@ -92,7 +92,10 @@ def _make_marker_array(self) -> MarkerArray: if isinstance(geom, MeshVisualShape): msg.type = Marker.MESH_RESOURCE msg.mesh_resource = "file://" + geom.file_name - msg.scale = Vector3(1, 1, 1) + if hasattr(geom, "scale") and geom.scale is not None: + msg.scale = Vector3(*geom.scale) + else: + msg.scale = Vector3(1, 1, 1) msg.mesh_use_embedded_materials = True elif isinstance(geom, CylinderVisualShape): msg.type = Marker.CYLINDER diff --git a/test/testRandom.py b/test/testRandom.py index 7777d15d7..2cd2d2d09 100644 --- a/test/testRandom.py +++ b/test/testRandom.py @@ -1,6 +1,8 @@ +import math + import pycrap from pycram.testing import BulletWorldTestCase -from pycram.process_module import ProcessModuleManager, ProcessModule +from pycram.process_module import ProcessModuleManager, ProcessModule, real_robot from pycram.robot_description import RobotDescriptionManager from pycram.datastructures.enums import ExecutionType from pycram.ros.ros_tools import sleep @@ -34,22 +36,50 @@ class ICUBTestCase(BulletWorldTestCase): @classmethod def setUpClass(cls): cls.world = BulletWorld(mode=WorldMode.GUI) - cls.milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) + # cls.milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) cls.robot = Object("icub", pycrap.Robot, "icub" + cls.extension, pose=Pose([0, 0, 0.5])) cls.kitchen = Object("kitchen", pycrap.Kitchen, "kitchen" + cls.extension) - cls.cereal = Object("cereal", pycrap.Cereal, "breakfast_cereal.stl", - pose=Pose([1.3, 0.7, 0.95])) + # cls.cereal = Object("cereal", pycrap.Cereal, "breakfast_cereal.stl", + # pose=Pose([1.3, 0.7, 0.95])) ProcessModule.execution_delay = False ProcessModuleManager.execution_type = ExecutionType.SIMULATED cls.viz_marker_publisher = VizMarkerPublisher() - def test_move_joints(self): + def test_move_joints_simulated(self): with simulated_robot: - MoveJointsMotion(["torso_roll"], [-20.0]).perform() - MoveArmJointsMotion(left_arm_poses={"r_elbow":20.0}).perform() - MoveArmJointsMotion(right_arm_poses={"r_elbow": 20.0}).perform() + MoveJointsMotion(["torso_roll"], [math.radians(-20.0)]).perform() + sleep(3) + ParkArmsActionPerformable(arm=Arms.BOTH).perform() + sleep(3) + MoveArmJointsMotion(left_arm_poses={"l_elbow": math.radians(20.0)}).perform() + sleep(3) + MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() + sleep(3) + LookingMotion(Pose([-2, -2, 3])).perform() + sleep(100) + + + + + def test_move_joints(self): + with real_robot: + MoveJointsMotion(["torso_roll"], [math.radians(20.0)]).perform() + sleep(3) + ParkArmsActionPerformable(arm=Arms.BOTH).perform() + sleep(3) + MoveArmJointsMotion(left_arm_poses={"l_elbow": math.radians(20.0)}).perform() + sleep(3) + MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() + LookingMotion(Pose([-2, -2, 3])).perform() + sleep(3) + + + def test_park_arms(self): + with real_robot: + ParkArmsActionPerformable(arm=Arms.BOTH).perform() + def test_try_world(self): - sleep(10) + sleep(100) # run_look_and_move_tcp() From 6083ad786ada1b0dc6883827c5b98904063edd95 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Fri, 6 Dec 2024 12:20:29 +0100 Subject: [PATCH 10/22] [iCub] updating real robot states using yarp updater --- .../process_modules/icub_process_modules.py | 52 ++++--------------- src/pycram/robot_description.py | 14 +++++ .../robot_descriptions/icub_description.py | 2 + test/testRandom.py | 14 ++++- 4 files changed, 39 insertions(+), 43 deletions(-) diff --git a/src/pycram/process_modules/icub_process_modules.py b/src/pycram/process_modules/icub_process_modules.py index 0a980a0d2..5ee709be6 100644 --- a/src/pycram/process_modules/icub_process_modules.py +++ b/src/pycram/process_modules/icub_process_modules.py @@ -1,16 +1,12 @@ import math from threading import Lock from typing import Union - from typing_extensions import Any - import actionlib - from .default_process_modules import DefaultMoveJoints, DefaultMoveArmJoints, DefaultMoveTCP, DefaultNavigation, \ DefaultMoveHead, DefaultDetecting, DefaultMoveGripper from .. import world_reasoning as btr import numpy as np - from ..process_module import ProcessModule, ProcessModuleManager from ..external_interfaces.ik import request_ik from ..ros.logging import logdebug @@ -27,36 +23,10 @@ from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType from ..external_interfaces import giskard from ..external_interfaces.robokudo import * - - import yarp -ACK_VOCAB = yarp.createVocab32('a','c','k') -NO_ACK_VOCAB = yarp.createVocab32('n','a','c','k') - - -def init_yarp_network(): - if not yarp.Network.checkNetwork(): - print("Unable to find a yarp server exiting ...") - return False - yarp.Network.init() - return True - -def open_rpc_client_port(port_name): - handle_port: yarp.RpcClient = yarp.RpcClient() - if not handle_port.open(port_name): - print(f"Can't open the port %s correctly" % port_name) - return False , None - print(f"Port %s opened correctly" % port_name) - return True , handle_port - -def open_buffered_bottle_port(port_name): - opened_port: yarp.BufferedPortBottle = yarp.BufferedPortBottle() - if not opened_port.open(port_name): - print(f"Can't open the port %s correctly" % port_name) - return False , None - print(f"Port %s opened correctly" % port_name) - return True , opened_port +from pycram.yarp_utils.yarp_networking import NO_ACK_VOCAB, ACK_VOCAB, open_rpc_client_port, open_buffered_bottle_port, \ + init_yarp_network def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): @@ -448,9 +418,9 @@ def __init__(self): self.ctp_right_arm_client_port_name = "/pycram/ctp/right_arm:oi" self.ctp_left_arm_client_port_name = "/pycram/ctp/left_arm:oi" - self.state_torso_port_name = "/pycram/ctp/torso:i" - self.state_right_arm_port_name = "/pycram/ctp/right_arm:i" - self.state_left_arm_port_name = "/pycram/ctp/left_arm:i" + self.state_torso_port_name = "/pycram/state/torso:i" + self.state_right_arm_port_name = "/pycram/state/right_arm:i" + self.state_left_arm_port_name = "/pycram/state/left_arm:i" self.gaze_client_port = None self.action_client_port = None @@ -597,33 +567,33 @@ def connect_yarp_ports(self)->bool: #connection_status = yarp.NetworkBase_connect(self.ctp_torso_client_port_name, "/testrpc", "tcp") if not connection_status: - print("action control port couldn't connect") + print("ctp torso control port couldn't connect") return False connection_status = yarp.NetworkBase_connect(self.ctp_right_arm_client_port_name, "/ctpservice/right_arm/rpc", "tcp") if not connection_status: - print("action control port couldn't connect") + print("ctp right_arm control port couldn't connect") return False connection_status = yarp.NetworkBase_connect( self.ctp_left_arm_client_port_name,"/ctpservice/left_arm/rpc", "tcp") if not connection_status: - print("action control port couldn't connect") + print("ctp left_arm control port couldn't connect") return False # status ports connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/torso/state:o", self.state_torso_port_name, "tcp") if not connection_status: - print("action control port couldn't connect") + print("state torso port couldn't connect") return False connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/right_arm/state:o",self.state_right_arm_port_name, "tcp") if not connection_status: - print("action control port couldn't connect") + print("state right_arm port couldn't connect") return False connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/left_arm/state:o",self.state_left_arm_port_name , "tcp") if not connection_status: - print("action control port couldn't connect") + print("state left_arm port couldn't connect") return False return True diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index ece78951d..bcb261f24 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -173,6 +173,20 @@ def get_actuator_for_joint(self, joint: str) -> Optional[Union[str, List[str]]]: return self.joint_actuators.get(joint) return None + + def get_actuated_joint_names(self, part_name) -> Optional[str]: + """ + Given a part name, returns the actuated joints. + + Args: + part_name (str): part name + + Returns: + List of actuated joint names: or None + """ + + return self.joint_actuators.get(part_name) + def get_actuated_joint_indices(self, joint_name) -> Tuple[Optional[str],Optional[int], Optional[int]]: """ Given a joint name, returns the indices of the part and the joint within that part. diff --git a/src/pycram/robot_descriptions/icub_description.py b/src/pycram/robot_descriptions/icub_description.py index d6b24192f..68e112f7e 100644 --- a/src/pycram/robot_descriptions/icub_description.py +++ b/src/pycram/robot_descriptions/icub_description.py @@ -29,6 +29,8 @@ "l_index_proximal","l_index_distal", "l_middle_proximal","l_middle_distal", "l_pinky"], + "head":["neck_pitch","neck_roll","neck_yaw", + "eyes_tilt","eyes_version","eyes_vergence"], } diff --git a/test/testRandom.py b/test/testRandom.py index 2cd2d2d09..76f252dfc 100644 --- a/test/testRandom.py +++ b/test/testRandom.py @@ -14,6 +14,8 @@ from pycram.datastructures.pose import Pose from pycram.process_module import simulated_robot from pycram.world_concepts.world_object import Object +from pycram.yarp_utils.yarp_joint_state_updater import run_icub_state_updater +from pycram.yarp_utils.yarp_networking import interrupt_and_close def run_look_and_move_tcp(): @@ -62,8 +64,13 @@ def test_move_joints_simulated(self): def test_move_joints(self): + + with real_robot: - MoveJointsMotion(["torso_roll"], [math.radians(20.0)]).perform() + yarpModule = run_icub_state_updater(['/', '--robot', 'icubSim']) + if yarpModule is None: + print("Failed to open yarp module") + MoveJointsMotion(["torso_roll"], [math.radians(-20.0)]).perform() sleep(3) ParkArmsActionPerformable(arm=Arms.BOTH).perform() sleep(3) @@ -71,7 +78,10 @@ def test_move_joints(self): sleep(3) MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() LookingMotion(Pose([-2, -2, 3])).perform() - sleep(3) + sleep(10) + if yarpModule is None: + print("stopping the module") + interrupt_and_close(yarpModule) def test_park_arms(self): From 087d86d3f8b408b74612fd4bafa5cd1e50085682 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Fri, 6 Dec 2024 19:55:23 +0100 Subject: [PATCH 11/22] [iCub] updating real robot states using yarp updater with fingers --- .../yarp_utils/yarp_joint_state_updater.py | 337 ++++++++++++++++++ src/pycram/yarp_utils/yarp_networking.py | 36 ++ test/testRandom.py | 14 +- 3 files changed, 382 insertions(+), 5 deletions(-) create mode 100644 src/pycram/yarp_utils/yarp_joint_state_updater.py create mode 100644 src/pycram/yarp_utils/yarp_networking.py diff --git a/src/pycram/yarp_utils/yarp_joint_state_updater.py b/src/pycram/yarp_utils/yarp_joint_state_updater.py new file mode 100644 index 000000000..995c634ea --- /dev/null +++ b/src/pycram/yarp_utils/yarp_joint_state_updater.py @@ -0,0 +1,337 @@ +import math +import sys +from time import sleep + +from pycram import World +from pycram.robot_description import RobotDescription +from pycram.yarp_utils.yarp_networking import interrupt_and_close +import yarp + + +class icub_state_updater(yarp.RFModule): + + def __init__(self): + yarp.RFModule.__init__(self) + + # handle port for the RFModule + self.handle_port = yarp.Port() + self.attach(self.handle_port) + + # Define vars to receive an image + self.module_name = None + self.robot_name_yarp = None + + self.state_torso_port_name = None + self.state_right_arm_port_name = None + self.state_left_arm_port_name = None + self.state_head_port_name = None + + self.torso_joints_name = None + self.right_arm_joints_name = None + self.left_arm_joints_name = None + self.head_joints_name = None + + self.state_torso_port = yarp.BufferedPortBottle() + self.state_right_arm_port = yarp.BufferedPortBottle() + self.state_left_arm_port = yarp.BufferedPortBottle() + self.state_head_port = yarp.BufferedPortBottle() + + def configure(self, rf): + self.module_name = rf.check("name", + yarp.Value("pycramICubStatePublisher"), + "module name (string)").asString() + + self.robot_name_yarp = rf.check("robot", + yarp.Value("icub"), + "module name (string)").asString() + + self.state_torso_port_name = self.getName("/state/torso:i") + self.state_right_arm_port_name = self.getName("/state/right_arm:i") + self.state_left_arm_port_name = self.getName("/state/left_arm:i") + self.state_head_port_name = self.getName("/state/head:i") + + + # Create handle port to read message + if not self.handle_port.open('/' + self.module_name): + print("Can't open the port correctly") + return False + + if not self.state_torso_port.open(self.state_torso_port_name): + print(f'Can\'t open the port correctly {self.state_torso_port_name}') + return False + + if not self.state_right_arm_port.open(self.state_right_arm_port_name): + print(f'Can\'t open the port correctly {self.state_right_arm_port_name}') + return False + + if not self.state_left_arm_port.open(self.state_left_arm_port_name): + print(f'Can\'t open the port correctly {self.state_left_arm_port_name}') + return False + + if not self.state_head_port.open(self.state_head_port_name): + print(f'Can\'t open the port correctly {self.state_head_port_name}') + return False + if not self.connect_ports(): + print("Error in connecting ports. Make sure that the robot is already up") + return False + + + self.torso_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("torso") + self.right_arm_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("right_arm") + self.left_arm_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("left_arm") + self.head_joints_names = ["neck_pitch","neck_roll","neck_yaw","eyes_tilt","r_eye_pan_joint","l_eye_pan_joint"] + + if self.torso_joints_names is None: + print("Error in joint names") + return False + + if self.right_arm_joints_names is None: + print("Error in joint names") + return False + + if self.left_arm_joints_names is None: + print("Error in joint names") + return False + + if self.head_joints_names is None: + print("Error in joint names") + return False + + print("Initialization complete") + return True + + def getName(self, name): + return f'/{self.module_name}{name}' + + + def connect_ports(self): + # status ports + connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/torso/state:o", self.state_torso_port_name, "tcp") + if not connection_status: + print("state torso port couldn't connect") + return False + + connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/right_arm/state:o",self.state_right_arm_port_name, "tcp") + if not connection_status: + print("state right_arm port couldn't connect") + return False + + connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/left_arm/state:o",self.state_left_arm_port_name , "tcp") + if not connection_status: + print("state left_arm port couldn't connect") + return False + + connection_status = yarp.NetworkBase_connect("/" + self.robot_name_yarp + "/head/state:o", + self.state_head_port_name, "tcp") + if not connection_status: + print("state head port couldn't connect") + return False + + return True + + def interruptModule(self): + print("stopping the module") + self.handle_port.interrupt() + self.state_torso_port.interrupt() + self.state_right_arm_port.interrupt() + self.state_left_arm_port.interrupt() + self.state_head_port.interrupt() + + return True + + def close(self): + self.handle_port.close() + self.state_torso_port.close() + self.state_right_arm_port.close() + self.state_left_arm_port.close() + self.state_head_port.close() + + return True + + def respond(self, command, reply): + + # Is the command recognized + + reply.clear() + + if command.get(0).asString() == "quit": + reply.addString("quitting") + return False + else: + reply.addString("other request") + + return True + + + def getPeriod(self): + """ + Module refresh rate. + Returns : The period of the module in seconds. + """ + return 0.5 + + def update_part_state(self,part_bottle : yarp.Bottle, part_names: [str]): + if part_bottle.size() == len(part_names): + for i in range(part_bottle.size()): + try: + World.robot.set_joint_position(part_names[i], + math.radians(part_bottle.get(i).asFloat64())) + except: + print("error in updating ",part_names[i]) + pass + + else : + print("mismatch in part size" , part_bottle.size() , " ", len(part_names)) + + def update_head_state(self,head_bottle : yarp.Bottle, head_names: [str]): + + for i in range(4): + try: + World.robot.set_joint_position(head_names[i], + math.radians(head_bottle.get(i).asFloat64())) + except: + print("error in updating head parts ", head_names[i]) + pass + + + try: + r_eye_pan = 0.5 * head_bottle.get(4).asFloat64() - 0.5 * head_bottle.get(5).asFloat64() + l_eye_pan = 0.5 * head_bottle.get(4).asFloat64() + 0.5 * head_bottle.get(5).asFloat64() + + World.robot.set_joint_position(head_names[4], + math.radians(r_eye_pan)) + + World.robot.set_joint_position(head_names[5], + math.radians(l_eye_pan)) + except: + print("error in updating head parts eyes") + pass + + def update_arm_state(self,arm_bottle : yarp.Bottle, arms_names: [str],arm:str): + try: + print("setting arm joints") + for i in range(7): + World.robot.set_joint_position(arms_names[i], + math.radians(arm_bottle.get(i).asFloat64())) + print(f"set {arms_names[i]} to {math.radians(arm_bottle.get(i).asFloat64())}") + print("arm joints updated") + #################################################################################### + #################################################################################### + World.robot.set_joint_position(f"{arm}_hand_index_0_joint", + math.radians(arm_bottle.get(7).asFloat64()/-3.0)) + World.robot.set_joint_position(f"{arm}_hand_middle_0_joint", + 0) + World.robot.set_joint_position(f"{arm}_hand_ring_0_joint", + math.radians(20 - (arm_bottle.get(7).asFloat64()/3.0))) + World.robot.set_joint_position(f"{arm}_hand_little_0_joint", + math.radians(20 - (arm_bottle.get(7).asFloat64()/3.0))) + + #################################################################################### + # thumb oppose (8) = 0 + print(f"setting thumb joint to {math.radians(arm_bottle.get(8).asFloat64())}") + World.robot.set_joint_position(f"{arm}_hand_thumb_0_joint", + math.radians(arm_bottle.get(8).asFloat64())) + print("thumb joint set") + + # # thumb proximal (9) = 1 + World.robot.set_joint_position(f"{arm}_hand_thumb_1_joint", + math.radians(arm_bottle.get(9).asFloat64())) + + # thumb distal (10) = 2 + 3 + World.robot.set_joint_position(f"{arm}_hand_thumb_2_joint", + math.radians(0.5*arm_bottle.get(10).asFloat64())) + World.robot.set_joint_position(f"{arm}_hand_thumb_3_joint", + math.radians(0.5*arm_bottle.get(10).asFloat64())) + #################################################################################### + # # index proximal (11) = 1 + World.robot.set_joint_position(f"{arm}_hand_index_1_joint", + math.radians(arm_bottle.get(11).asFloat64())) + + # index distal (12) = 2 + 3 + World.robot.set_joint_position(f"{arm}_hand_index_2_joint", + math.radians(0.5 * arm_bottle.get(12).asFloat64())) + World.robot.set_joint_position(f"{arm}_hand_index_3_joint", + math.radians(0.5 * arm_bottle.get(12).asFloat64())) + #################################################################################### + # middle proximal (13) = 1 + World.robot.set_joint_position(f"{arm}_hand_middle_1_joint", + math.radians(arm_bottle.get(13).asFloat64())) + + # middle distal (14) = 2 + 3 + World.robot.set_joint_position(f"{arm}_hand_middle_2_joint", + math.radians(0.5 * arm_bottle.get(14).asFloat64())) + World.robot.set_joint_position(f"{arm}_hand_middle_3_joint", + math.radians(0.5 * arm_bottle.get(14).asFloat64())) + #################################################################################### + + # Pinky (15) = 1 + 2 + 3 (little) (ring) + World.robot.set_joint_position(f"{arm}_hand_ring_1_joint", + math.radians(arm_bottle.get(15).asFloat64()/3.0)) + World.robot.set_joint_position(f"{arm}_hand_ring_2_joint", + math.radians(arm_bottle.get(15).asFloat64()/3.0)) + World.robot.set_joint_position(f"{arm}_hand_ring_3_joint", + math.radians( arm_bottle.get(15).asFloat64()/3.0)) + #################################################################################### + # Pinky (15) = 1 + 2 + 3 (little) (ring) + World.robot.set_joint_position(f"{arm}_hand_little_1_joint", + math.radians(arm_bottle.get(15).asFloat64()/3.0)) + World.robot.set_joint_position(f"{arm}_hand_little_2_joint", + math.radians(arm_bottle.get(15).asFloat64()/3.0)) + World.robot.set_joint_position(f"{arm}_hand_little_3_joint", + math.radians(arm_bottle.get(15).asFloat64()/3.0)) + #################################################################################### + except Exception as e: + print(e) + print("error in updating arm parts" , f"{arm}_hand_thumb_0_joint") + pass + + + + + def updateModule(self): + torso_state: yarp.Bottle = self.state_torso_port.read(shouldWait=False) + right_arm_state: yarp.Bottle = self.state_right_arm_port.read(shouldWait=False) + left_arm_state: yarp.Bottle = self.state_left_arm_port.read(shouldWait=False) + head_state: yarp.Bottle = self.state_head_port.read(shouldWait=False) + + if torso_state is not None: + self.update_part_state(torso_state,self.torso_joints_names) + + if right_arm_state is not None: + self.update_arm_state(right_arm_state,self.right_arm_joints_names,"r") + + if left_arm_state is not None: + self.update_arm_state(left_arm_state,self.left_arm_joints_names,"l") + + if head_state is not None: + self.update_head_state(head_state,self.head_joints_names) + + return True + + + + + +def run_icub_state_updater(args): + if not yarp.Network.checkNetwork(): + print("error in network check") + return + + m_module = icub_state_updater() + + rf = yarp.ResourceFinder() + rf.setVerbose(True) + rf.setDefaultContext('pycram') + #rf.setDefaultConfigFile('icub_joint_ipdater.ini') + + if rf.configure(args): + print("done configuration") + if m_module.runModuleThreaded(rf) == 0: + print("done running module") + return m_module + else: + interrupt_and_close(m_module) + print("Module Failed to open") + return None + + diff --git a/src/pycram/yarp_utils/yarp_networking.py b/src/pycram/yarp_utils/yarp_networking.py new file mode 100644 index 000000000..23ec0ec0e --- /dev/null +++ b/src/pycram/yarp_utils/yarp_networking.py @@ -0,0 +1,36 @@ + +import yarp + + +ACK_VOCAB = yarp.createVocab32('a','c','k') +NO_ACK_VOCAB = yarp.createVocab32('n','a','c','k') + + +def init_yarp_network(): + if not yarp.Network.checkNetwork(): + print("Unable to find a yarp server exiting ...") + return False + + yarp.Network.init() + return True + +def open_rpc_client_port(port_name): + handle_port: yarp.RpcClient = yarp.RpcClient() + if not handle_port.open(port_name): + print(f"Can't open the port %s correctly" % port_name) + return False , None + print(f"Port %s opened correctly" % port_name) + return True , handle_port + +def open_buffered_bottle_port(port_name): + opened_port: yarp.BufferedPortBottle = yarp.BufferedPortBottle() + if not opened_port.open(port_name): + print(f"Can't open the port %s correctly" % port_name) + return False , None + print(f"Port %s opened correctly" % port_name) + return True , opened_port + + +def interrupt_and_close(m_module:yarp.RFModule): + m_module.interruptModule() + m_module.close() diff --git a/test/testRandom.py b/test/testRandom.py index 76f252dfc..d91e4fc53 100644 --- a/test/testRandom.py +++ b/test/testRandom.py @@ -37,7 +37,7 @@ class ICUBTestCase(BulletWorldTestCase): @classmethod def setUpClass(cls): - cls.world = BulletWorld(mode=WorldMode.GUI) + cls.world = BulletWorld(mode=WorldMode.DIRECT) # cls.milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) cls.robot = Object("icub", pycrap.Robot, "icub" + cls.extension, pose=Pose([0, 0, 0.5])) cls.kitchen = Object("kitchen", pycrap.Kitchen, "kitchen" + cls.extension) @@ -47,6 +47,10 @@ def setUpClass(cls): ProcessModuleManager.execution_type = ExecutionType.SIMULATED cls.viz_marker_publisher = VizMarkerPublisher() + def test_move_joint_direct(self): + self.robot.set_joint_position("l_hand_thumb_0_joint",math.radians(30)) + + def test_move_joints_simulated(self): with simulated_robot: MoveJointsMotion(["torso_roll"], [math.radians(-20.0)]).perform() @@ -71,14 +75,14 @@ def test_move_joints(self): if yarpModule is None: print("Failed to open yarp module") MoveJointsMotion(["torso_roll"], [math.radians(-20.0)]).perform() - sleep(3) + ParkArmsActionPerformable(arm=Arms.BOTH).perform() - sleep(3) + MoveArmJointsMotion(left_arm_poses={"l_elbow": math.radians(20.0)}).perform() - sleep(3) + MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() LookingMotion(Pose([-2, -2, 3])).perform() - sleep(10) + if yarpModule is None: print("stopping the module") interrupt_and_close(yarpModule) From 05d791cd243de946aad03d3c30bb1b8599a10ab8 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Mon, 9 Dec 2024 16:36:02 +0100 Subject: [PATCH 12/22] [iCub] adding gripper close and open fuctionalities in both real and simulation --- .../process_modules/icub_process_modules.py | 67 +++++- .../robot_descriptions/icub_description.py | 83 ++++++++ .../yarp_utils/yarp_joint_state_updater.py | 191 ++++++++---------- test/testRandom.py | 21 +- 4 files changed, 255 insertions(+), 107 deletions(-) diff --git a/src/pycram/process_modules/icub_process_modules.py b/src/pycram/process_modules/icub_process_modules.py index 5ee709be6..3204f93a9 100644 --- a/src/pycram/process_modules/icub_process_modules.py +++ b/src/pycram/process_modules/icub_process_modules.py @@ -20,7 +20,7 @@ from ..datastructures.world import World from ..world_concepts.world_object import Object from ..datastructures.pose import Pose -from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType +from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType, GripperState from ..external_interfaces import giskard from ..external_interfaces.robokudo import * import yarp @@ -28,6 +28,33 @@ from pycram.yarp_utils.yarp_networking import NO_ACK_VOCAB, ACK_VOCAB, open_rpc_client_port, open_buffered_bottle_port, \ init_yarp_network +def update_hand(hand_values,ctp_port): + yarp_bottle_msg: yarp.Bottle = yarp.Bottle() + yarp_bottle_reply: yarp.Bottle = yarp.Bottle() + yarp_bottle_msg.addVocab32('c', 't', 'p', 'q') + yarp_bottle_msg.addVocab32('t', 'i', 'm', 'e') + yarp_bottle_msg.addFloat32(1.5) + yarp_bottle_msg.addVocab32('o', 'f', 'f') + yarp_bottle_msg.addInt32(7) + yarp_bottle_msg.addVocab32('p', 'o', 's') + target_loc: yarp.Bottle = yarp_bottle_msg.addList() + for i in hand_values: + target_loc.addFloat32(i) + + print("command Ready to send to iCub part tcp") + + ctp_port.write(yarp_bottle_msg, yarp_bottle_reply) + reply_vocab = yarp_bottle_reply.get(0).asVocab32() + + if reply_vocab == NO_ACK_VOCAB: + print("NO_ACK") + return False + elif reply_vocab == ACK_VOCAB: + print("ACK") + return True + else: + print("another reply") + return False def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): if len(joint_to_change_idx): @@ -67,6 +94,8 @@ def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): print("another reply") return False +HAND_CLOSED = [60.0,60.0,0,85.0,20,85,20,85,85] +HAND_OPENED = [0 ,0 ,0,0 ,0 ,0 ,0 ,0 ,0] class iCubNavigationReal(ProcessModule): """ The process module to move the robot from one position to another. @@ -126,9 +155,39 @@ class iCubMoveGripperReal(ProcessModule): This process module controls the gripper of the robot. They can either be opened or closed. Furthermore, it can only moved one gripper at a time. """ + def __init__(self, lock, + state_ports : [yarp.BufferedPortBottle], + ctp_ports: [yarp.RpcClient]): + super().__init__(lock) + self.state_ports = state_ports + self.ctp_ports = ctp_ports + print("iCubMoveGripperReal initialized") + def _execute(self, desig: MoveGripperMotion): - print("iCub Move Gripper") + gripper = desig.gripper + required_status = desig.motion + if gripper == Arms.RIGHT: + if required_status == GripperState.CLOSE: + update_hand(HAND_CLOSED,self.ctp_ports[0]) + else: + update_hand(HAND_OPENED, self.ctp_ports[0]) + + elif gripper == Arms.LEFT: + if required_status == GripperState.CLOSE: + update_hand(HAND_CLOSED, self.ctp_ports[1]) + else: + update_hand(HAND_OPENED, self.ctp_ports[1]) + + elif gripper ==Arms.BOTH: + if required_status == GripperState.CLOSE: + update_hand(HAND_CLOSED, self.ctp_ports[0]) + update_hand(HAND_CLOSED, self.ctp_ports[1]) + else: + update_hand(HAND_OPENED, self.ctp_ports[0]) + update_hand(HAND_OPENED, self.ctp_ports[1]) + else: + print ("undefined arm") class iCubDetectingReal(ProcessModule): @@ -499,7 +558,9 @@ def move_gripper(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return iCubMoveGripper(self._move_gripper_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: - return iCubMoveGripperReal(self._move_gripper_lock) + return iCubMoveGripperReal(self._move_gripper_lock, + [self.state_right_arm_port, self.state_left_arm_port], + [self.ctp_right_arm_client_port,self.ctp_left_arm_client_port]) def open(self): print("iCub doesn't perform open action from here") diff --git a/src/pycram/robot_descriptions/icub_description.py b/src/pycram/robot_descriptions/icub_description.py index 68e112f7e..ecb84e37e 100644 --- a/src/pycram/robot_descriptions/icub_description.py +++ b/src/pycram/robot_descriptions/icub_description.py @@ -60,6 +60,48 @@ left_gripper = EndEffectorDescription("left_gripper", "l_hand", "l_hand", icub_description.urdf_object) +left_gripper.add_static_joint_states(GripperState.CLOSE, {'l_hand_index_0_joint': math.radians(-20.0), + 'l_hand_index_1_joint': math.radians(20.0), + 'l_hand_index_2_joint': math.radians(42.5), + 'l_hand_index_3_joint': math.radians(42.5), + 'l_hand_little_0_joint': math.radians(0.0), + 'l_hand_little_1_joint': math.radians(28.0), + 'l_hand_little_2_joint': math.radians(28.0), + 'l_hand_little_3_joint': math.radians(28.0), + 'l_hand_middle_0_joint': math.radians(0.0), + 'l_hand_middle_1_joint': math.radians(20.0), + 'l_hand_middle_2_joint': math.radians(42.5), + 'l_hand_middle_3_joint': math.radians(42.5), + 'l_hand_ring_0_joint': math.radians(0.0), + 'l_hand_ring_1_joint': math.radians(28.0), + 'l_hand_ring_2_joint': math.radians(28.0), + 'l_hand_ring_3_joint': math.radians(28.0), + 'l_hand_thumb_0_joint': math.radians(60.0), + 'l_hand_thumb_1_joint': math.radians(0.0), + 'l_hand_thumb_2_joint': math.radians(42.5), + 'l_hand_thumb_3_joint': math.radians(42.5)}) + +left_gripper.add_static_joint_states(GripperState.OPEN, {'l_hand_index_0_joint': math.radians(0.0), + 'l_hand_index_1_joint': math.radians(0.0), + 'l_hand_index_2_joint': math.radians(0.0), + 'l_hand_index_3_joint': math.radians(0.0), + 'l_hand_little_0_joint': math.radians(20.0), + 'l_hand_little_1_joint': math.radians(0.0), + 'l_hand_little_2_joint': math.radians(0.0), + 'l_hand_little_3_joint': math.radians(0.0), + 'l_hand_middle_0_joint': math.radians(0.0), + 'l_hand_middle_1_joint': math.radians(0.0), + 'l_hand_middle_2_joint': math.radians(0.0), + 'l_hand_middle_3_joint': math.radians(0.0), + 'l_hand_ring_0_joint': math.radians(20.0), + 'l_hand_ring_1_joint': math.radians(0.0), + 'l_hand_ring_2_joint': math.radians(0.0), + 'l_hand_ring_3_joint': math.radians(0.0), + 'l_hand_thumb_0_joint': math.radians(0.0), + 'l_hand_thumb_1_joint': math.radians(0.0), + 'l_hand_thumb_2_joint': math.radians(0.0), + 'l_hand_thumb_3_joint': math.radians(0.0)}) + left_arm.end_effector = left_gripper @@ -83,6 +125,47 @@ icub_description.urdf_object) +right_gripper.add_static_joint_states(GripperState.CLOSE, {'r_hand_index_0_joint': math.radians(-20.0), + 'r_hand_index_1_joint': math.radians(20.0), + 'r_hand_index_2_joint': math.radians(42.5), + 'r_hand_index_3_joint': math.radians(42.5), + 'r_hand_little_0_joint': math.radians(0.0), + 'r_hand_little_1_joint': math.radians(28.0), + 'r_hand_little_2_joint': math.radians(28.0), + 'r_hand_little_3_joint': math.radians(28.0), + 'r_hand_middle_0_joint': math.radians(0.0), + 'r_hand_middle_1_joint': math.radians(20.0), + 'r_hand_middle_2_joint': math.radians(42.5), + 'r_hand_middle_3_joint': math.radians(42.5), + 'r_hand_ring_0_joint': math.radians(0.0), + 'r_hand_ring_1_joint': math.radians(28.0), + 'r_hand_ring_2_joint': math.radians(28.0), + 'r_hand_ring_3_joint': math.radians(28.0), + 'r_hand_thumb_0_joint': math.radians(60.0), + 'r_hand_thumb_1_joint': math.radians(0.0), + 'r_hand_thumb_2_joint': math.radians(42.5), + 'r_hand_thumb_3_joint': math.radians(42.5)}) + +right_gripper.add_static_joint_states(GripperState.OPEN, {'r_hand_index_0_joint': math.radians(0.0), + 'r_hand_index_1_joint': math.radians(0.0), + 'r_hand_index_2_joint': math.radians(0.0), + 'r_hand_index_3_joint': math.radians(0.0), + 'r_hand_little_0_joint': math.radians(20.0), + 'r_hand_little_1_joint': math.radians(0.0), + 'r_hand_little_2_joint': math.radians(0.0), + 'r_hand_little_3_joint': math.radians(0.0), + 'r_hand_middle_0_joint': math.radians(0.0), + 'r_hand_middle_1_joint': math.radians(0.0), + 'r_hand_middle_2_joint': math.radians(0.0), + 'r_hand_middle_3_joint': math.radians(0.0), + 'r_hand_ring_0_joint': math.radians(20.0), + 'r_hand_ring_1_joint': math.radians(0.0), + 'r_hand_ring_2_joint': math.radians(0.0), + 'r_hand_ring_3_joint': math.radians(0.0), + 'r_hand_thumb_0_joint': math.radians(0.0), + 'r_hand_thumb_1_joint': math.radians(0.0), + 'r_hand_thumb_2_joint': math.radians(0.0), + 'r_hand_thumb_3_joint': math.radians(0.0)}) right_arm.end_effector = right_gripper diff --git a/src/pycram/yarp_utils/yarp_joint_state_updater.py b/src/pycram/yarp_utils/yarp_joint_state_updater.py index 995c634ea..00f12af8f 100644 --- a/src/pycram/yarp_utils/yarp_joint_state_updater.py +++ b/src/pycram/yarp_utils/yarp_joint_state_updater.py @@ -170,120 +170,107 @@ def getPeriod(self): """ return 0.5 - def update_part_state(self,part_bottle : yarp.Bottle, part_names: [str]): + def update_joint_degree(self,joint_name,joint_value_degree): + try: + World.robot.set_joint_position(joint_name,math.radians(joint_value_degree)) + + except Exception as e: + print(f"error in updating joint {joint_name} to {joint_value_degree} degree") + pass + + + + def update_torso_state(self, part_bottle : yarp.Bottle, part_names: [str]): if part_bottle.size() == len(part_names): for i in range(part_bottle.size()): - try: - World.robot.set_joint_position(part_names[i], - math.radians(part_bottle.get(i).asFloat64())) - except: - print("error in updating ",part_names[i]) - pass - + self.update_joint_degree(part_names[i], + part_bottle.get(i).asFloat64()) else : print("mismatch in part size" , part_bottle.size() , " ", len(part_names)) def update_head_state(self,head_bottle : yarp.Bottle, head_names: [str]): - for i in range(4): - try: - World.robot.set_joint_position(head_names[i], - math.radians(head_bottle.get(i).asFloat64())) - except: - print("error in updating head parts ", head_names[i]) - pass + self.update_joint_degree(head_names[i], + head_bottle.get(i).asFloat64()) - try: - r_eye_pan = 0.5 * head_bottle.get(4).asFloat64() - 0.5 * head_bottle.get(5).asFloat64() - l_eye_pan = 0.5 * head_bottle.get(4).asFloat64() + 0.5 * head_bottle.get(5).asFloat64() + r_eye_pan = 0.5 * head_bottle.get(4).asFloat64() - 0.5 * head_bottle.get(5).asFloat64() + l_eye_pan = 0.5 * head_bottle.get(4).asFloat64() + 0.5 * head_bottle.get(5).asFloat64() + + self.update_joint_degree(head_names[4],r_eye_pan) + self.update_joint_degree(head_names[5],l_eye_pan) - World.robot.set_joint_position(head_names[4], - math.radians(r_eye_pan)) - World.robot.set_joint_position(head_names[5], - math.radians(l_eye_pan)) - except: - print("error in updating head parts eyes") - pass def update_arm_state(self,arm_bottle : yarp.Bottle, arms_names: [str],arm:str): - try: - print("setting arm joints") - for i in range(7): - World.robot.set_joint_position(arms_names[i], - math.radians(arm_bottle.get(i).asFloat64())) - print(f"set {arms_names[i]} to {math.radians(arm_bottle.get(i).asFloat64())}") - print("arm joints updated") - #################################################################################### - #################################################################################### - World.robot.set_joint_position(f"{arm}_hand_index_0_joint", - math.radians(arm_bottle.get(7).asFloat64()/-3.0)) - World.robot.set_joint_position(f"{arm}_hand_middle_0_joint", - 0) - World.robot.set_joint_position(f"{arm}_hand_ring_0_joint", - math.radians(20 - (arm_bottle.get(7).asFloat64()/3.0))) - World.robot.set_joint_position(f"{arm}_hand_little_0_joint", - math.radians(20 - (arm_bottle.get(7).asFloat64()/3.0))) - - #################################################################################### - # thumb oppose (8) = 0 - print(f"setting thumb joint to {math.radians(arm_bottle.get(8).asFloat64())}") - World.robot.set_joint_position(f"{arm}_hand_thumb_0_joint", - math.radians(arm_bottle.get(8).asFloat64())) - print("thumb joint set") - - # # thumb proximal (9) = 1 - World.robot.set_joint_position(f"{arm}_hand_thumb_1_joint", - math.radians(arm_bottle.get(9).asFloat64())) - - # thumb distal (10) = 2 + 3 - World.robot.set_joint_position(f"{arm}_hand_thumb_2_joint", - math.radians(0.5*arm_bottle.get(10).asFloat64())) - World.robot.set_joint_position(f"{arm}_hand_thumb_3_joint", - math.radians(0.5*arm_bottle.get(10).asFloat64())) - #################################################################################### - # # index proximal (11) = 1 - World.robot.set_joint_position(f"{arm}_hand_index_1_joint", - math.radians(arm_bottle.get(11).asFloat64())) - - # index distal (12) = 2 + 3 - World.robot.set_joint_position(f"{arm}_hand_index_2_joint", - math.radians(0.5 * arm_bottle.get(12).asFloat64())) - World.robot.set_joint_position(f"{arm}_hand_index_3_joint", - math.radians(0.5 * arm_bottle.get(12).asFloat64())) - #################################################################################### - # middle proximal (13) = 1 - World.robot.set_joint_position(f"{arm}_hand_middle_1_joint", - math.radians(arm_bottle.get(13).asFloat64())) - - # middle distal (14) = 2 + 3 - World.robot.set_joint_position(f"{arm}_hand_middle_2_joint", - math.radians(0.5 * arm_bottle.get(14).asFloat64())) - World.robot.set_joint_position(f"{arm}_hand_middle_3_joint", - math.radians(0.5 * arm_bottle.get(14).asFloat64())) - #################################################################################### - - # Pinky (15) = 1 + 2 + 3 (little) (ring) - World.robot.set_joint_position(f"{arm}_hand_ring_1_joint", - math.radians(arm_bottle.get(15).asFloat64()/3.0)) - World.robot.set_joint_position(f"{arm}_hand_ring_2_joint", - math.radians(arm_bottle.get(15).asFloat64()/3.0)) - World.robot.set_joint_position(f"{arm}_hand_ring_3_joint", - math.radians( arm_bottle.get(15).asFloat64()/3.0)) - #################################################################################### - # Pinky (15) = 1 + 2 + 3 (little) (ring) - World.robot.set_joint_position(f"{arm}_hand_little_1_joint", - math.radians(arm_bottle.get(15).asFloat64()/3.0)) - World.robot.set_joint_position(f"{arm}_hand_little_2_joint", - math.radians(arm_bottle.get(15).asFloat64()/3.0)) - World.robot.set_joint_position(f"{arm}_hand_little_3_joint", - math.radians(arm_bottle.get(15).asFloat64()/3.0)) - #################################################################################### - except Exception as e: - print(e) - print("error in updating arm parts" , f"{arm}_hand_thumb_0_joint") - pass + for i in range(7): + self.update_joint_degree(arms_names[i],arm_bottle.get(i).asFloat64()) + #################################################################################### + #################################################################################### + # hand (7) = index 0 + ring 0 + little 0 + self.update_joint_degree(f"{arm}_hand_index_0_joint", + arm_bottle.get(7).asFloat64()/-3.0) + self.update_joint_degree(f"{arm}_hand_middle_0_joint", + 0) + self.update_joint_degree(f"{arm}_hand_ring_0_joint", + 20 - (arm_bottle.get(7).asFloat64()/3.0)) + self.update_joint_degree(f"{arm}_hand_little_0_joint", + 20 - (arm_bottle.get(7).asFloat64()/3.0)) + + #################################################################################### + # thumb oppose (8) = 0 + + self.update_joint_degree(f"{arm}_hand_thumb_0_joint", + arm_bottle.get(8).asFloat64()) + + # # thumb proximal (9) = 1 + self.update_joint_degree(f"{arm}_hand_thumb_1_joint", + arm_bottle.get(9).asFloat64()) + + # thumb distal (10) = 2 + 3 + self.update_joint_degree(f"{arm}_hand_thumb_2_joint", + 0.5*arm_bottle.get(10).asFloat64()) + self.update_joint_degree(f"{arm}_hand_thumb_3_joint", + 0.5*arm_bottle.get(10).asFloat64()) + #################################################################################### + # # index proximal (11) = 1 + self.update_joint_degree(f"{arm}_hand_index_1_joint", + arm_bottle.get(11).asFloat64()) + + # index distal (12) = 2 + 3 + self.update_joint_degree(f"{arm}_hand_index_2_joint", + 0.5 * arm_bottle.get(12).asFloat64()) + self.update_joint_degree(f"{arm}_hand_index_3_joint", + 0.5 * arm_bottle.get(12).asFloat64()) + #################################################################################### + # middle proximal (13) = 1 + self.update_joint_degree(f"{arm}_hand_middle_1_joint", + arm_bottle.get(13).asFloat64()) + + # middle distal (14) = 2 + 3 + self.update_joint_degree(f"{arm}_hand_middle_2_joint", + 0.5 * arm_bottle.get(14).asFloat64()) + self.update_joint_degree(f"{arm}_hand_middle_3_joint", + 0.5 * arm_bottle.get(14).asFloat64()) + #################################################################################### + + # Pinky (15) = 1 + 2 + 3 (little) (ring) + self.update_joint_degree(f"{arm}_hand_ring_1_joint", + arm_bottle.get(15).asFloat64()/3.0) + self.update_joint_degree(f"{arm}_hand_ring_2_joint", + arm_bottle.get(15).asFloat64()/3.0) + self.update_joint_degree(f"{arm}_hand_ring_3_joint", + arm_bottle.get(15).asFloat64()/3.0) + #################################################################################### + # Pinky (15) = 1 + 2 + 3 (little) (ring) + self.update_joint_degree(f"{arm}_hand_little_1_joint", + arm_bottle.get(15).asFloat64()/3.0) + self.update_joint_degree(f"{arm}_hand_little_2_joint", + arm_bottle.get(15).asFloat64()/3.0) + self.update_joint_degree(f"{arm}_hand_little_3_joint", + arm_bottle.get(15).asFloat64()/3.0) + #################################################################################### + @@ -295,7 +282,7 @@ def updateModule(self): head_state: yarp.Bottle = self.state_head_port.read(shouldWait=False) if torso_state is not None: - self.update_part_state(torso_state,self.torso_joints_names) + self.update_torso_state(torso_state, self.torso_joints_names) if right_arm_state is not None: self.update_arm_state(right_arm_state,self.right_arm_joints_names,"r") diff --git a/test/testRandom.py b/test/testRandom.py index d91e4fc53..048291156 100644 --- a/test/testRandom.py +++ b/test/testRandom.py @@ -10,7 +10,7 @@ from pycram.worlds.bullet_world import BulletWorld from pycram.designators.action_designator import * from pycram.designators.location_designator import * -from pycram.datastructures.enums import WorldMode +from pycram.datastructures.enums import WorldMode , GripperState from pycram.datastructures.pose import Pose from pycram.process_module import simulated_robot from pycram.world_concepts.world_object import Object @@ -62,7 +62,16 @@ def test_move_joints_simulated(self): MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() sleep(3) LookingMotion(Pose([-2, -2, 3])).perform() - sleep(100) + sleep(3) + MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() + MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() + sleep(20) + MoveGripperMotion(GripperState.CLOSE, Arms.RIGHT).perform() + MoveGripperMotion(GripperState.CLOSE, Arms.LEFT).perform() + sleep(10) + MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() + MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() + @@ -82,6 +91,14 @@ def test_move_joints(self): MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() LookingMotion(Pose([-2, -2, 3])).perform() + MoveGripperMotion(GripperState.OPEN,Arms.LEFT).perform() + sleep(10) + MoveGripperMotion(GripperState.CLOSE,Arms.LEFT).perform() + MoveGripperMotion(GripperState.CLOSE, Arms.RIGHT).perform() + sleep(10) + MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() + MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() + sleep(10) if yarpModule is None: print("stopping the module") From be8500b3ba8168cd49408120d8e68ac0ae5d4b4a Mon Sep 17 00:00:00 2001 From: oeldardear Date: Mon, 9 Dec 2024 16:51:56 +0100 Subject: [PATCH 13/22] [iCub] rename test --- test/testRandom.py | 116 ------------------------------------------- test/test_icub.py | 120 +++++++++++++++++++++++++++++++++++++++++---- 2 files changed, 111 insertions(+), 125 deletions(-) delete mode 100644 test/testRandom.py diff --git a/test/testRandom.py b/test/testRandom.py deleted file mode 100644 index 048291156..000000000 --- a/test/testRandom.py +++ /dev/null @@ -1,116 +0,0 @@ -import math - -import pycrap -from pycram.testing import BulletWorldTestCase -from pycram.process_module import ProcessModuleManager, ProcessModule, real_robot -from pycram.robot_description import RobotDescriptionManager -from pycram.datastructures.enums import ExecutionType -from pycram.ros.ros_tools import sleep -from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher -from pycram.worlds.bullet_world import BulletWorld -from pycram.designators.action_designator import * -from pycram.designators.location_designator import * -from pycram.datastructures.enums import WorldMode , GripperState -from pycram.datastructures.pose import Pose -from pycram.process_module import simulated_robot -from pycram.world_concepts.world_object import Object -from pycram.yarp_utils.yarp_joint_state_updater import run_icub_state_updater -from pycram.yarp_utils.yarp_networking import interrupt_and_close - - -def run_look_and_move_tcp(): - rm = RobotDescriptionManager() - rm.load_description('icub') - - with simulated_robot: - pm = ProcessModuleManager.get_manager() - - LookingMotion(Pose([-2, -2, 3])).perform() - time.sleep(2) - MoveTCPMotion(Pose([-0.2, 0.1, 0.1]), Arms.LEFT).perform() - MoveJointsMotion(["torso_roll"], [20.0]).perform() - - pm.exit() - - -class ICUBTestCase(BulletWorldTestCase): - - @classmethod - def setUpClass(cls): - cls.world = BulletWorld(mode=WorldMode.DIRECT) - # cls.milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cls.robot = Object("icub", pycrap.Robot, "icub" + cls.extension, pose=Pose([0, 0, 0.5])) - cls.kitchen = Object("kitchen", pycrap.Kitchen, "kitchen" + cls.extension) - # cls.cereal = Object("cereal", pycrap.Cereal, "breakfast_cereal.stl", - # pose=Pose([1.3, 0.7, 0.95])) - ProcessModule.execution_delay = False - ProcessModuleManager.execution_type = ExecutionType.SIMULATED - cls.viz_marker_publisher = VizMarkerPublisher() - - def test_move_joint_direct(self): - self.robot.set_joint_position("l_hand_thumb_0_joint",math.radians(30)) - - - def test_move_joints_simulated(self): - with simulated_robot: - MoveJointsMotion(["torso_roll"], [math.radians(-20.0)]).perform() - sleep(3) - ParkArmsActionPerformable(arm=Arms.BOTH).perform() - sleep(3) - MoveArmJointsMotion(left_arm_poses={"l_elbow": math.radians(20.0)}).perform() - sleep(3) - MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() - sleep(3) - LookingMotion(Pose([-2, -2, 3])).perform() - sleep(3) - MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() - MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() - sleep(20) - MoveGripperMotion(GripperState.CLOSE, Arms.RIGHT).perform() - MoveGripperMotion(GripperState.CLOSE, Arms.LEFT).perform() - sleep(10) - MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() - MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() - - - - - - def test_move_joints(self): - - - with real_robot: - yarpModule = run_icub_state_updater(['/', '--robot', 'icubSim']) - if yarpModule is None: - print("Failed to open yarp module") - MoveJointsMotion(["torso_roll"], [math.radians(-20.0)]).perform() - - ParkArmsActionPerformable(arm=Arms.BOTH).perform() - - MoveArmJointsMotion(left_arm_poses={"l_elbow": math.radians(20.0)}).perform() - - MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() - LookingMotion(Pose([-2, -2, 3])).perform() - MoveGripperMotion(GripperState.OPEN,Arms.LEFT).perform() - sleep(10) - MoveGripperMotion(GripperState.CLOSE,Arms.LEFT).perform() - MoveGripperMotion(GripperState.CLOSE, Arms.RIGHT).perform() - sleep(10) - MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() - MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() - sleep(10) - - if yarpModule is None: - print("stopping the module") - interrupt_and_close(yarpModule) - - - def test_park_arms(self): - with real_robot: - ParkArmsActionPerformable(arm=Arms.BOTH).perform() - - - def test_try_world(self): - sleep(100) - -# run_look_and_move_tcp() diff --git a/test/test_icub.py b/test/test_icub.py index 5cfd1d64e..048291156 100644 --- a/test/test_icub.py +++ b/test/test_icub.py @@ -1,14 +1,116 @@ +import math + +import pycrap +from pycram.testing import BulletWorldTestCase +from pycram.process_module import ProcessModuleManager, ProcessModule, real_robot +from pycram.robot_description import RobotDescriptionManager +from pycram.datastructures.enums import ExecutionType +from pycram.ros.ros_tools import sleep +from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher +from pycram.worlds.bullet_world import BulletWorld +from pycram.designators.action_designator import * +from pycram.designators.location_designator import * +from pycram.datastructures.enums import WorldMode , GripperState from pycram.datastructures.pose import Pose -from pycram.designators.motion_designator import LookingMotion -from pycram.process_module import ProcessModuleManager, simulated_robot -from pycram.robot_description import RobotDescription, RobotDescriptionManager +from pycram.process_module import simulated_robot +from pycram.world_concepts.world_object import Object +from pycram.yarp_utils.yarp_joint_state_updater import run_icub_state_updater +from pycram.yarp_utils.yarp_networking import interrupt_and_close + + +def run_look_and_move_tcp(): + rm = RobotDescriptionManager() + rm.load_description('icub') + + with simulated_robot: + pm = ProcessModuleManager.get_manager() + + LookingMotion(Pose([-2, -2, 3])).perform() + time.sleep(2) + MoveTCPMotion(Pose([-0.2, 0.1, 0.1]), Arms.LEFT).perform() + MoveJointsMotion(["torso_roll"], [20.0]).perform() + + pm.exit() + + +class ICUBTestCase(BulletWorldTestCase): + + @classmethod + def setUpClass(cls): + cls.world = BulletWorld(mode=WorldMode.DIRECT) + # cls.milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) + cls.robot = Object("icub", pycrap.Robot, "icub" + cls.extension, pose=Pose([0, 0, 0.5])) + cls.kitchen = Object("kitchen", pycrap.Kitchen, "kitchen" + cls.extension) + # cls.cereal = Object("cereal", pycrap.Cereal, "breakfast_cereal.stl", + # pose=Pose([1.3, 0.7, 0.95])) + ProcessModule.execution_delay = False + ProcessModuleManager.execution_type = ExecutionType.SIMULATED + cls.viz_marker_publisher = VizMarkerPublisher() + + def test_move_joint_direct(self): + self.robot.set_joint_position("l_hand_thumb_0_joint",math.radians(30)) + + + def test_move_joints_simulated(self): + with simulated_robot: + MoveJointsMotion(["torso_roll"], [math.radians(-20.0)]).perform() + sleep(3) + ParkArmsActionPerformable(arm=Arms.BOTH).perform() + sleep(3) + MoveArmJointsMotion(left_arm_poses={"l_elbow": math.radians(20.0)}).perform() + sleep(3) + MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() + sleep(3) + LookingMotion(Pose([-2, -2, 3])).perform() + sleep(3) + MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() + MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() + sleep(20) + MoveGripperMotion(GripperState.CLOSE, Arms.RIGHT).perform() + MoveGripperMotion(GripperState.CLOSE, Arms.LEFT).perform() + sleep(10) + MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() + MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() + + + + + + def test_move_joints(self): + + + with real_robot: + yarpModule = run_icub_state_updater(['/', '--robot', 'icubSim']) + if yarpModule is None: + print("Failed to open yarp module") + MoveJointsMotion(["torso_roll"], [math.radians(-20.0)]).perform() + + ParkArmsActionPerformable(arm=Arms.BOTH).perform() + + MoveArmJointsMotion(left_arm_poses={"l_elbow": math.radians(20.0)}).perform() + + MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() + LookingMotion(Pose([-2, -2, 3])).perform() + MoveGripperMotion(GripperState.OPEN,Arms.LEFT).perform() + sleep(10) + MoveGripperMotion(GripperState.CLOSE,Arms.LEFT).perform() + MoveGripperMotion(GripperState.CLOSE, Arms.RIGHT).perform() + sleep(10) + MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() + MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() + sleep(10) + + if yarpModule is None: + print("stopping the module") + interrupt_and_close(yarpModule) + -rm = RobotDescriptionManager() -rm.load_description('iCub') + def test_park_arms(self): + with real_robot: + ParkArmsActionPerformable(arm=Arms.BOTH).perform() -with simulated_robot: - pm = ProcessModuleManager.get_manager() - LookingMotion(Pose([-2, -2, 3])).perform() + def test_try_world(self): + sleep(100) - pm.exit() +# run_look_and_move_tcp() From 9b62853026837bd4a8ba980eaa9f44bc7c2f0620 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Mon, 9 Dec 2024 16:55:21 +0100 Subject: [PATCH 14/22] clean up --- src/pycram/process_modules/iCubYarpTest.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) delete mode 100644 src/pycram/process_modules/iCubYarpTest.py diff --git a/src/pycram/process_modules/iCubYarpTest.py b/src/pycram/process_modules/iCubYarpTest.py deleted file mode 100644 index e69de29bb..000000000 From 7ba39af228a712cb8528e22e59763ebbda969dc1 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Tue, 10 Dec 2024 15:16:07 +0100 Subject: [PATCH 15/22] [iCub] adding failure and logging correctly --- src/pycram/failures.py | 10 ++ .../process_modules/icub_process_modules.py | 155 ++++++++++-------- .../yarp_utils/yarp_joint_state_updater.py | 148 +++++++++++------ test/test_icub.py | 8 +- 4 files changed, 197 insertions(+), 124 deletions(-) diff --git a/src/pycram/failures.py b/src/pycram/failures.py index 8a621536b..f8ef0b886 100644 --- a/src/pycram/failures.py +++ b/src/pycram/failures.py @@ -506,3 +506,13 @@ class LinkGeometryHasNoMesh(Exception): def __init__(self, link_name: str, geometry_type: str): super().__init__(f"Link {link_name} geometry with type {geometry_type} has no mesh.") + +class YarpNetworkError(Exception): + def __init__(self): + super().__init__(f"Yarp Network Initialization Failed. Check if yarpserver is already up") + + +class RobotNotInitialized(Exception): + def __init__(self,robot_name: str): + super().__init__(f"Robot ({robot_name} not correctly initialized") + diff --git a/src/pycram/process_modules/icub_process_modules.py b/src/pycram/process_modules/icub_process_modules.py index 3204f93a9..be9c84b82 100644 --- a/src/pycram/process_modules/icub_process_modules.py +++ b/src/pycram/process_modules/icub_process_modules.py @@ -7,9 +7,11 @@ DefaultMoveHead, DefaultDetecting, DefaultMoveGripper from .. import world_reasoning as btr import numpy as np + +from ..failures import RobotNotInitialized from ..process_module import ProcessModule, ProcessModuleManager from ..external_interfaces.ik import request_ik -from ..ros.logging import logdebug +from ..ros.logging import logdebug, logerr from ..utils import _apply_ik from ..local_transformer import LocalTransformer from ..designators.object_designator import ObjectDesignatorDescription @@ -23,10 +25,14 @@ from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType, GripperState from ..external_interfaces import giskard from ..external_interfaces.robokudo import * -import yarp -from pycram.yarp_utils.yarp_networking import NO_ACK_VOCAB, ACK_VOCAB, open_rpc_client_port, open_buffered_bottle_port, \ - init_yarp_network +try: + import yarp + from pycram.yarp_utils.yarp_networking import * +except ImportError: + logwarn("Yarp Was Not Found. You can't use iCub. Check yarp installation") + yarp = None + pass def update_hand(hand_values,ctp_port): yarp_bottle_msg: yarp.Bottle = yarp.Bottle() @@ -41,19 +47,18 @@ def update_hand(hand_values,ctp_port): for i in hand_values: target_loc.addFloat32(i) - print("command Ready to send to iCub part tcp") ctp_port.write(yarp_bottle_msg, yarp_bottle_reply) reply_vocab = yarp_bottle_reply.get(0).asVocab32() if reply_vocab == NO_ACK_VOCAB: - print("NO_ACK") + loginfo("Received: NO_ACK") return False elif reply_vocab == ACK_VOCAB: - print("ACK") + loginfo("Received: ACK") return True else: - print("another reply") + loginfo("Received: another reply") return False def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): @@ -61,7 +66,6 @@ def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): part_state: yarp.Bottle = state_port.read(shouldWait=True) part_new_states = [] for i in range(part_state.size()): - print(i, " ", part_state.get(i).asFloat32()) part_new_states.append(part_state.get(i).asFloat32()) for i in range(len(joint_to_change_idx)): @@ -79,19 +83,17 @@ def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): for i in part_new_states: target_loc.addFloat32(i) - print("command Ready to send to iCub part tcp") - ctp_port.write(yarp_bottle_msg, yarp_bottle_reply) reply_vocab = yarp_bottle_reply.get(0).asVocab32() if reply_vocab == NO_ACK_VOCAB: - print("NO_ACK") + loginfo("Received: NO_ACK") return False elif reply_vocab == ACK_VOCAB: - print("ACK") + loginfo("Received: ACK") return True else: - print("another reply") + loginfo("Received: another reply") return False HAND_CLOSED = [60.0,60.0,0,85.0,20,85,20,85,85] @@ -102,7 +104,7 @@ class iCubNavigationReal(ProcessModule): """ def _execute(self, desig: MoveMotion): - print("iCubNavigate") + loginfo("iCubNavigationReal function. No implementation ATM") class iCubMoveHeadReal(ProcessModule): @@ -116,7 +118,7 @@ def __init__(self,lock,cmd_port:yarp.RpcClient): self.cmd_port = cmd_port def _execute(self, desig: LookingMotion): - print("iCub Move Head") + loginfo("iCubMoveHeadReal") position_target = desig.target.pose.position if self.cmd_port.getOutputCount(): @@ -129,23 +131,22 @@ def _execute(self, desig: LookingMotion): target_loc.addFloat32(position_target.x) target_loc.addFloat32(position_target.y) target_loc.addFloat32(position_target.z) - print(f"command Ready to send to iCub") self.cmd_port .write(yarp_bottle_msg, yarp_bottle_reply) reply_vocab = yarp_bottle_reply.get(0).asVocab32() if reply_vocab == NO_ACK_VOCAB: - print("NO_ACK") + loginfo("Received: NO_ACK") return False elif reply_vocab == ACK_VOCAB: - print("ACK") + loginfo("Received: ACK") return True else: - print("another reply") + loginfo("Received: another reply") return False else: - print("port is not connected") + logwarn("control port is not connected") return False @@ -161,7 +162,7 @@ def __init__(self, lock, super().__init__(lock) self.state_ports = state_ports self.ctp_ports = ctp_ports - print("iCubMoveGripperReal initialized") + loginfo("iCubMoveGripperReal initialized") def _execute(self, desig: MoveGripperMotion): @@ -187,7 +188,7 @@ def _execute(self, desig: MoveGripperMotion): update_hand(HAND_OPENED, self.ctp_ports[0]) update_hand(HAND_OPENED, self.ctp_ports[1]) else: - print ("undefined arm") + logwarn ("undefined arm") class iCubDetectingReal(ProcessModule): @@ -197,7 +198,7 @@ class iCubDetectingReal(ProcessModule): """ def _execute(self, desig: DetectingMotion): - print("iCub Detect") + loginfo("iCubDetectingReal. No Implementation ATM") class iCubMoveTCPReal(ProcessModule): """ @@ -209,7 +210,7 @@ def __init__(self, lock, cmd_port: yarp.RpcClient): self.cmd_port = cmd_port def _execute(self, desig: MoveTCPMotion): - print("iCub Move Head") + loginfo("iCubMoveTCPReal") position_target = desig.target.position if self.cmd_port.getOutputCount(): @@ -221,7 +222,7 @@ def _execute(self, desig: MoveTCPMotion): target_loc.addFloat32(position_target.x) target_loc.addFloat32(position_target.y) target_loc.addFloat32(position_target.z) - print(f"command Ready to send to iCub") + yarp_bottle_msg.addString("side") if desig.arm == Arms.LEFT: yarp_bottle_msg.addString("left") @@ -234,18 +235,18 @@ def _execute(self, desig: MoveTCPMotion): reply_vocab = yarp_bottle_reply.get(0).asVocab32() if reply_vocab == NO_ACK_VOCAB: - print("NO_ACK") + loginfo("Received: NO_ACK") return False elif reply_vocab == ACK_VOCAB: - print("ACK") + loginfo("Received: ACK") return True else: - print("another reply") + loginfo("Received: another reply") return False else: - print("port is not connected") + logwarn("port is not connected") return False @@ -263,7 +264,7 @@ def __init__(self, lock, super().__init__(lock) self.state_ports = state_ports self.ctp_ports = ctp_ports - print("iCubMoveArmJoints initialized") + loginfo("iCubMoveArmJointsReal initialized") def _execute(self, desig: MoveArmJointsMotion): @@ -285,9 +286,9 @@ def _execute(self, desig: MoveArmJointsMotion): right_arm_to_change_joints.append(joint_index) right_arm_to_change_joints_states.append(joint_pose) else: - print("error in joint name") + logwarn("error in joint name. (Not part of right arm chain)") else: - print("error in joint name") + logwarn("error in joint name") if left_arm_to_change is not None: for joint_mame, joint_pose in left_arm_to_change.items(): part_name,part_index, joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices( @@ -297,9 +298,9 @@ def _execute(self, desig: MoveArmJointsMotion): left_arm_to_change_joints.append(joint_index) left_arm_to_change_joints_states.append(joint_pose) else: - print("error in joint name") + logwarn("error in joint name. (Not part of left arm chain)") else: - print("error in joint name") + logwarn("error in joint name") update_part(self.state_ports[0], @@ -314,8 +315,6 @@ def _execute(self, desig: MoveArmJointsMotion): - print("iCub Move Arm Joints") - class iCubMoveJointsReal(ProcessModule): """ Process Module for generic joint movements, is not confined to the arms but can move any joint of the robot @@ -327,6 +326,7 @@ def __init__(self, lock, super().__init__(lock) self.state_ports = state_ports self.ctp_ports = ctp_ports + loginfo("iCubMoveJointsReal initialized") @@ -356,7 +356,7 @@ def _execute(self, desig: MoveJointsMotion): left_arm_to_change_joints.append(joint_index) left_arm_to_change_joints_states.append(to_change_states[index]) else: - print("error in index") + logwarn("error in index") index += 1 @@ -377,7 +377,6 @@ def _execute(self, desig: MoveJointsMotion): - print("iCub Move Joints") class iCubWorldStateDetecting(ProcessModule): @@ -466,6 +465,13 @@ def __init__(self): self._move_gripper_lock = Lock() self._open_lock = Lock() self._close_lock = Lock() + + self.initialized = False + + if yarp is None: + logwarn("Yarp not Found. ICubManager wasn't initialized correctly") + return + self.yarp_network_state = init_yarp_network() # yarp related @@ -491,44 +497,46 @@ def __init__(self): self.state_torso_port = yarp.BufferedPortBottle() self.state_right_arm_port = yarp.BufferedPortBottle() self.state_left_arm_port = yarp.BufferedPortBottle() - self.initialized = False + if self.yarp_network_state: - print("yarp network state detected") + loginfo("yarp network state detected") self.config_yarp_ports() self.connect_yarp_ports() self.initialized = True else: - print("yarp network state not detected") - - - - - + logwarn("yarp network state not detected. ICubManager wasn't initialized correctly") def navigate(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: - print('Navigate iCub') return iCubNavigation(self._navigate_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: + if not self.initialized: + raise RobotNotInitialized() return iCubNavigationReal(self._navigate_lock) def looking(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return iCubMoveHead(self._looking_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: + if not self.initialized: + raise RobotNotInitialized() return iCubMoveHeadReal(self._looking_lock,self.gaze_client_port) def detecting(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return iCubDetecting(self._detecting_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: + if not self.initialized: + raise RobotNotInitialized() return iCubDetectingReal(self._detecting_lock) def move_tcp(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return iCubMoveTCP(self._move_tcp_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: + if not self.initialized: + raise RobotNotInitialized() return iCubMoveTCPReal(self._move_tcp_lock,self.action_client_port) def move_arm_joints(self): @@ -536,6 +544,8 @@ def move_arm_joints(self): return iCubMoveArmJoints(self._move_arm_joints_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: + if not self.initialized: + raise RobotNotInitialized() return iCubMoveArmJointsReal(self._move_arm_joints_lock, [self.state_right_arm_port, self.state_left_arm_port], [self.ctp_right_arm_client_port,self.ctp_left_arm_client_port]) @@ -550,6 +560,8 @@ def move_joints(self): return iCubMoveJoints(self._move_joints_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: + if not self.initialized: + raise RobotNotInitialized() return iCubMoveJointsReal(self._move_joints_lock, [self.state_torso_port,self.state_right_arm_port,self.state_left_arm_port], [self.ctp_torso_client_port,self.ctp_right_arm_client_port,self.ctp_left_arm_client_port]) @@ -558,103 +570,102 @@ def move_gripper(self): if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: return iCubMoveGripper(self._move_gripper_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: + if not self.initialized: + raise RobotNotInitialized() return iCubMoveGripperReal(self._move_gripper_lock, [self.state_right_arm_port, self.state_left_arm_port], [self.ctp_right_arm_client_port,self.ctp_left_arm_client_port]) def open(self): - print("iCub doesn't perform open action from here") + logwarn("iCub doesn't perform open action from here") def close(self): - print("iCub doesn't perform close action from here") + logwarn("iCub doesn't perform close action from here") def config_yarp_ports(self)->bool: suc, self.gaze_client_port = open_rpc_client_port(self.gaze_cmd_port_name) if not suc: - print("Failed to open, ", self.gaze_cmd_port_name) + logerr(f"Failed to open {self.gaze_cmd_port_name}") return False suc, self.action_client_port = open_rpc_client_port(self.action_cmd_port_name) if not suc: - print("Failed to open, ", self.action_cmd_port_name) + logerr(f"Failed to open {self.action_cmd_port_name}") return False suc, self.ctp_torso_client_port = open_rpc_client_port(self.ctp_torso_client_port_name) if not suc: - print("Failed to open, ", self.ctp_torso_client_port_name) + logerr(f"Failed to open {self.ctp_torso_client_port_name}") return False suc, self.ctp_right_arm_client_port = open_rpc_client_port(self.ctp_right_arm_client_port_name) if not suc: - print("Failed to open, ", self.ctp_right_arm_client_port_name) + logerr(f"Failed to open {self.ctp_right_arm_client_port_name}") return False suc, self.ctp_left_arm_client_port = open_rpc_client_port(self.ctp_left_arm_client_port_name) if not suc: - print("Failed to open, ", self.ctp_left_arm_client_port_name) + logerr(f"Failed to open {self.ctp_left_arm_client_port_name}") return False suc, self.state_torso_port = open_buffered_bottle_port(self.state_torso_port_name) if not suc: - print("Failed to open, ", self.state_torso_port_name) + logerr(f"Failed to open {self.state_torso_port_name}") return False suc, self.state_right_arm_port = open_buffered_bottle_port(self.state_right_arm_port_name) if not suc: - print("Failed to open, ", self.state_right_arm_port_name) + logerr(f"Failed to open {self.state_right_arm_port_name}") return False suc, self.state_left_arm_port = open_buffered_bottle_port(self.state_left_arm_port_name) if not suc: - print("Failed to open, ", self.state_left_arm_port_name) + logerr(f"Failed to open {self.state_left_arm_port_name}") return False - - return True def connect_yarp_ports(self)->bool: connection_status = yarp.NetworkBase_connect(self.gaze_cmd_port_name, "/iKinGazeCtrl/rpc", "tcp") if not connection_status: - print("gaze control port couldn't connect") + logerr("gaze control port couldn't connect") connection_status = yarp.NetworkBase_connect(self.action_cmd_port_name, "/actionsRenderingEngine/cmd:io", "tcp") if not connection_status: - print("action control port couldn't connect") + logerr("action control port couldn't connect") return False # ctp service ports connection_status = yarp.NetworkBase_connect(self.ctp_torso_client_port_name, "/ctpservice/torso/rpc", "tcp") - #connection_status = yarp.NetworkBase_connect(self.ctp_torso_client_port_name, "/testrpc", "tcp") if not connection_status: - print("ctp torso control port couldn't connect") + logerr("ctp torso control port couldn't connect") return False connection_status = yarp.NetworkBase_connect(self.ctp_right_arm_client_port_name, "/ctpservice/right_arm/rpc", "tcp") if not connection_status: - print("ctp right_arm control port couldn't connect") + logerr("ctp right_arm control port couldn't connect") return False connection_status = yarp.NetworkBase_connect( self.ctp_left_arm_client_port_name,"/ctpservice/left_arm/rpc", "tcp") if not connection_status: - print("ctp left_arm control port couldn't connect") + logerr("ctp left_arm control port couldn't connect") return False # status ports connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/torso/state:o", self.state_torso_port_name, "tcp") if not connection_status: - print("state torso port couldn't connect") + logerr("state torso port couldn't connect") return False connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/right_arm/state:o",self.state_right_arm_port_name, "tcp") if not connection_status: - print("state right_arm port couldn't connect") + logerr("state right_arm port couldn't connect") return False connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/left_arm/state:o",self.state_left_arm_port_name , "tcp") if not connection_status: - print("state left_arm port couldn't connect") + logerr("state left_arm port couldn't connect") return False return True @@ -666,7 +677,8 @@ def exit(self): def disconnect_and_remove(self): - self.gaze_client_port .interrupt() + loginfo("interrupting yarp ports") + self.gaze_client_port.interrupt() self.action_client_port.interrupt() self.ctp_torso_client_port.interrupt() self.ctp_right_arm_client_port.interrupt() @@ -675,7 +687,8 @@ def disconnect_and_remove(self): self.state_right_arm_port.interrupt() self.state_left_arm_port.interrupt() - self.gaze_client_port .close() + loginfo("closing yarp ports") + self.gaze_client_port.close() self.action_client_port.close() self.ctp_torso_client_port.close() self.ctp_right_arm_client_port.close() diff --git a/src/pycram/yarp_utils/yarp_joint_state_updater.py b/src/pycram/yarp_utils/yarp_joint_state_updater.py index 00f12af8f..929ac7690 100644 --- a/src/pycram/yarp_utils/yarp_joint_state_updater.py +++ b/src/pycram/yarp_utils/yarp_joint_state_updater.py @@ -3,21 +3,23 @@ from time import sleep from pycram import World +from pycram.failures import YarpNetworkError from pycram.robot_description import RobotDescription -from pycram.yarp_utils.yarp_networking import interrupt_and_close +from pycram.ros.logging import logdebug, logwarn, loginfo, logerr +from pycram.yarp_utils.yarp_networking import * import yarp -class icub_state_updater(yarp.RFModule): +class IcubStateUpdater(yarp.RFModule): def __init__(self): yarp.RFModule.__init__(self) - # handle port for the RFModule + self.handle_port = yarp.Port() self.attach(self.handle_port) - # Define vars to receive an image + self.module_name = None self.robot_name_yarp = None @@ -26,16 +28,16 @@ def __init__(self): self.state_left_arm_port_name = None self.state_head_port_name = None - self.torso_joints_name = None - self.right_arm_joints_name = None - self.left_arm_joints_name = None - self.head_joints_name = None - self.state_torso_port = yarp.BufferedPortBottle() self.state_right_arm_port = yarp.BufferedPortBottle() self.state_left_arm_port = yarp.BufferedPortBottle() self.state_head_port = yarp.BufferedPortBottle() + self.torso_joints_names = None + self.right_arm_joints_names = None + self.left_arm_joints_names = None + self.head_joints_names = None + def configure(self, rf): self.module_name = rf.check("name", yarp.Value("pycramICubStatePublisher"), @@ -53,26 +55,26 @@ def configure(self, rf): # Create handle port to read message if not self.handle_port.open('/' + self.module_name): - print("Can't open the port correctly") + logerr("Can't open the port correctly") return False if not self.state_torso_port.open(self.state_torso_port_name): - print(f'Can\'t open the port correctly {self.state_torso_port_name}') + logerr(f'Can\'t open the port correctly {self.state_torso_port_name}') return False if not self.state_right_arm_port.open(self.state_right_arm_port_name): - print(f'Can\'t open the port correctly {self.state_right_arm_port_name}') + logerr(f'Can\'t open the port correctly {self.state_right_arm_port_name}') return False if not self.state_left_arm_port.open(self.state_left_arm_port_name): - print(f'Can\'t open the port correctly {self.state_left_arm_port_name}') + logerr(f'Can\'t open the port correctly {self.state_left_arm_port_name}') return False if not self.state_head_port.open(self.state_head_port_name): - print(f'Can\'t open the port correctly {self.state_head_port_name}') + logerr(f'Can\'t open the port correctly {self.state_head_port_name}') return False if not self.connect_ports(): - print("Error in connecting ports. Make sure that the robot is already up") + logerr("Error in connecting ports. Make sure that the robot is already up") return False @@ -81,56 +83,83 @@ def configure(self, rf): self.left_arm_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("left_arm") self.head_joints_names = ["neck_pitch","neck_roll","neck_yaw","eyes_tilt","r_eye_pan_joint","l_eye_pan_joint"] - if self.torso_joints_names is None: - print("Error in joint names") - return False - - if self.right_arm_joints_names is None: - print("Error in joint names") - return False + loginfo("Initialization complete") + return True - if self.left_arm_joints_names is None: - print("Error in joint names") - return False + def getName(self, name): + """ + Constructs a full YARP-compatible port name for the module. - if self.head_joints_names is None: - print("Error in joint names") - return False + This method appends a given string (`name`) to the module's base name + (`self.module_name`) to construct a complete port name in the YARP + naming convention. The port name is returned as a formatted string + prefixed with a forward slash (`/`), which is the standard for YARP ports. - print("Initialization complete") - return True + Args: + name (str): The specific name to append to the module's base name. - def getName(self, name): + Returns: + str: The fully constructed YARP-compatible port name. + """ return f'/{self.module_name}{name}' def connect_ports(self): - # status ports + """ + Establishes connections between the robot's state output ports and the module's state input ports. + + This method uses the YARP (Yet Another Robot Platform) networking library to connect the robot's + state output ports (torso, right arm, left arm, head) to the corresponding state input ports in + the module. The connections are established using the TCP protocol to ensure reliable communication. + + If any connection fails, an error message is printed to indicate which port could not connect, + and the method returns `False`. If all connections are successfully established, the method + returns `True`. + + Returns: + bool: `True` if all ports successfully connect, `False` otherwise. + """ + # Torso state port connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/torso/state:o", self.state_torso_port_name, "tcp") if not connection_status: - print("state torso port couldn't connect") + logdebug("state torso port couldn't connect") return False + # Right arm state port connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/right_arm/state:o",self.state_right_arm_port_name, "tcp") if not connection_status: - print("state right_arm port couldn't connect") + logdebug("state right_arm port couldn't connect") return False + # Left Arm state port connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/left_arm/state:o",self.state_left_arm_port_name , "tcp") if not connection_status: - print("state left_arm port couldn't connect") + logdebug("state left_arm port couldn't connect") return False + # Head state port connection_status = yarp.NetworkBase_connect("/" + self.robot_name_yarp + "/head/state:o", self.state_head_port_name, "tcp") if not connection_status: - print("state head port couldn't connect") + logdebug("state head port couldn't connect") return False return True def interruptModule(self): - print("stopping the module") + + """ + Interrupts all communication ports associated with this object. + + This method is used to temporarily halt the operation of the module by + interrupting the communication ports. Interrupting the ports allows the + system to pause ongoing communication or processing without permanently + closing the connections. + + Returns: + bool: `True` if all ports were successfully interrupted. + """ + self.handle_port.interrupt() self.state_torso_port.interrupt() self.state_right_arm_port.interrupt() @@ -140,6 +169,26 @@ def interruptModule(self): return True def close(self): + """ + Closes all the communication ports associated with this object. + + This method is responsible for ensuring that all the communication ports + used by the system are properly closed to free up resources and maintain + system integrity. The ports being closed include: + + - `handle_port`: The port used for handling general rpc commands for the yarp module. + - `state_torso_port`: The port used for monitoring the torso's state. + - `state_right_arm_port`: The port used for monitoring the right arm's state. + - `state_left_arm_port`: The port used for monitoring the left arm's state. + - `state_head_port`: The port used for monitoring the head's state. + + After closing all the ports, the function returns `True` to indicate + successful closure of all resources. + + Returns: + bool: `True` if all ports were successfully closed. + """ + self.handle_port.close() self.state_torso_port.close() self.state_right_arm_port.close() @@ -150,8 +199,12 @@ def close(self): def respond(self, command, reply): - # Is the command recognized + """ + Processes a command and generates an appropriate response. + This method handles incoming commands (RPC) through the port /module_name + Further it execute a behaviour and then replies + """ reply.clear() if command.get(0).asString() == "quit": @@ -168,14 +221,14 @@ def getPeriod(self): Module refresh rate. Returns : The period of the module in seconds. """ - return 0.5 + return 1 - def update_joint_degree(self,joint_name,joint_value_degree): + def update_joint_degree(self,joint_name:str,joint_value_degree:float): try: World.robot.set_joint_position(joint_name,math.radians(joint_value_degree)) except Exception as e: - print(f"error in updating joint {joint_name} to {joint_value_degree} degree") + logerr(f"error in updating joint {joint_name} to {joint_value_degree} degree") pass @@ -186,7 +239,7 @@ def update_torso_state(self, part_bottle : yarp.Bottle, part_names: [str]): self.update_joint_degree(part_names[i], part_bottle.get(i).asFloat64()) else : - print("mismatch in part size" , part_bottle.size() , " ", len(part_names)) + logwarn(f"mismatch in torso joint sizes {part_bottle.size()} , {len(part_names)}") def update_head_state(self,head_bottle : yarp.Bottle, head_names: [str]): for i in range(4): @@ -301,10 +354,9 @@ def updateModule(self): def run_icub_state_updater(args): if not yarp.Network.checkNetwork(): - print("error in network check") - return + raise YarpNetworkError() - m_module = icub_state_updater() + m_module = IcubStateUpdater() rf = yarp.ResourceFinder() rf.setVerbose(True) @@ -312,13 +364,13 @@ def run_icub_state_updater(args): #rf.setDefaultConfigFile('icub_joint_ipdater.ini') if rf.configure(args): - print("done configuration") + logdebug("icub_state_updater Configuration done") if m_module.runModuleThreaded(rf) == 0: - print("done running module") + loginfo("done running module") return m_module else: interrupt_and_close(m_module) - print("Module Failed to open") + logerr("Module Failed to open") return None diff --git a/test/test_icub.py b/test/test_icub.py index 048291156..6576b6fb8 100644 --- a/test/test_icub.py +++ b/test/test_icub.py @@ -77,8 +77,6 @@ def test_move_joints_simulated(self): def test_move_joints(self): - - with real_robot: yarpModule = run_icub_state_updater(['/', '--robot', 'icubSim']) if yarpModule is None: @@ -92,13 +90,13 @@ def test_move_joints(self): MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() LookingMotion(Pose([-2, -2, 3])).perform() MoveGripperMotion(GripperState.OPEN,Arms.LEFT).perform() - sleep(10) + sleep(2) MoveGripperMotion(GripperState.CLOSE,Arms.LEFT).perform() MoveGripperMotion(GripperState.CLOSE, Arms.RIGHT).perform() - sleep(10) + sleep(2) MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() - sleep(10) + sleep(2) if yarpModule is None: print("stopping the module") From 13e8c5be26f83b967fed968c1e15be537e3ab1dd Mon Sep 17 00:00:00 2001 From: oeldardear Date: Wed, 11 Dec 2024 09:50:51 +0100 Subject: [PATCH 16/22] [iCub] minor refactoring --- .../yarp_networking.py | 0 .../process_modules/icub_process_modules.py | 32 ++--- .../yarp_utils/yarp_joint_state_updater.py | 5 +- test/test_icub.py | 114 ------------------ 4 files changed, 10 insertions(+), 141 deletions(-) rename src/pycram/{yarp_utils => external_interfaces}/yarp_networking.py (100%) delete mode 100644 test/test_icub.py diff --git a/src/pycram/yarp_utils/yarp_networking.py b/src/pycram/external_interfaces/yarp_networking.py similarity index 100% rename from src/pycram/yarp_utils/yarp_networking.py rename to src/pycram/external_interfaces/yarp_networking.py diff --git a/src/pycram/process_modules/icub_process_modules.py b/src/pycram/process_modules/icub_process_modules.py index be9c84b82..44acef5a0 100644 --- a/src/pycram/process_modules/icub_process_modules.py +++ b/src/pycram/process_modules/icub_process_modules.py @@ -1,34 +1,20 @@ import math -from threading import Lock -from typing import Union -from typing_extensions import Any -import actionlib from .default_process_modules import DefaultMoveJoints, DefaultMoveArmJoints, DefaultMoveTCP, DefaultNavigation, \ DefaultMoveHead, DefaultDetecting, DefaultMoveGripper -from .. import world_reasoning as btr -import numpy as np from ..failures import RobotNotInitialized from ..process_module import ProcessModule, ProcessModuleManager -from ..external_interfaces.ik import request_ik -from ..ros.logging import logdebug, logerr -from ..utils import _apply_ik -from ..local_transformer import LocalTransformer -from ..designators.object_designator import ObjectDesignatorDescription +from ..ros.logging import logerr from ..designators.motion_designator import MoveMotion, LookingMotion, \ DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \ - MoveGripperMotion, OpeningMotion, ClosingMotion + MoveGripperMotion from ..robot_description import RobotDescription from ..datastructures.world import World -from ..world_concepts.world_object import Object -from ..datastructures.pose import Pose -from ..datastructures.enums import JointType, ObjectType, Arms, ExecutionType, GripperState -from ..external_interfaces import giskard +from ..datastructures.enums import Arms, ExecutionType, GripperState from ..external_interfaces.robokudo import * try: - import yarp - from pycram.yarp_utils.yarp_networking import * + from pycram.external_interfaces.yarp_networking import * except ImportError: logwarn("Yarp Was Not Found. You can't use iCub. Check yarp installation") yarp = None @@ -512,7 +498,7 @@ def navigate(self): return iCubNavigation(self._navigate_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: if not self.initialized: - raise RobotNotInitialized() + raise RobotNotInitialized("icub") return iCubNavigationReal(self._navigate_lock) def looking(self): @@ -520,7 +506,7 @@ def looking(self): return iCubMoveHead(self._looking_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: if not self.initialized: - raise RobotNotInitialized() + raise RobotNotInitialized("icub") return iCubMoveHeadReal(self._looking_lock,self.gaze_client_port) def detecting(self): @@ -528,7 +514,7 @@ def detecting(self): return iCubDetecting(self._detecting_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: if not self.initialized: - raise RobotNotInitialized() + raise RobotNotInitialized("icub") return iCubDetectingReal(self._detecting_lock) def move_tcp(self): @@ -536,7 +522,7 @@ def move_tcp(self): return iCubMoveTCP(self._move_tcp_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: if not self.initialized: - raise RobotNotInitialized() + raise RobotNotInitialized("icub") return iCubMoveTCPReal(self._move_tcp_lock,self.action_client_port) def move_arm_joints(self): @@ -545,7 +531,7 @@ def move_arm_joints(self): elif ProcessModuleManager.execution_type == ExecutionType.REAL: if not self.initialized: - raise RobotNotInitialized() + raise RobotNotInitialized("icub") return iCubMoveArmJointsReal(self._move_arm_joints_lock, [self.state_right_arm_port, self.state_left_arm_port], [self.ctp_right_arm_client_port,self.ctp_left_arm_client_port]) diff --git a/src/pycram/yarp_utils/yarp_joint_state_updater.py b/src/pycram/yarp_utils/yarp_joint_state_updater.py index 929ac7690..92df06acd 100644 --- a/src/pycram/yarp_utils/yarp_joint_state_updater.py +++ b/src/pycram/yarp_utils/yarp_joint_state_updater.py @@ -1,13 +1,10 @@ import math -import sys -from time import sleep from pycram import World from pycram.failures import YarpNetworkError from pycram.robot_description import RobotDescription from pycram.ros.logging import logdebug, logwarn, loginfo, logerr -from pycram.yarp_utils.yarp_networking import * -import yarp +from pycram.external_interfaces.yarp_networking import * class IcubStateUpdater(yarp.RFModule): diff --git a/test/test_icub.py b/test/test_icub.py deleted file mode 100644 index 6576b6fb8..000000000 --- a/test/test_icub.py +++ /dev/null @@ -1,114 +0,0 @@ -import math - -import pycrap -from pycram.testing import BulletWorldTestCase -from pycram.process_module import ProcessModuleManager, ProcessModule, real_robot -from pycram.robot_description import RobotDescriptionManager -from pycram.datastructures.enums import ExecutionType -from pycram.ros.ros_tools import sleep -from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher -from pycram.worlds.bullet_world import BulletWorld -from pycram.designators.action_designator import * -from pycram.designators.location_designator import * -from pycram.datastructures.enums import WorldMode , GripperState -from pycram.datastructures.pose import Pose -from pycram.process_module import simulated_robot -from pycram.world_concepts.world_object import Object -from pycram.yarp_utils.yarp_joint_state_updater import run_icub_state_updater -from pycram.yarp_utils.yarp_networking import interrupt_and_close - - -def run_look_and_move_tcp(): - rm = RobotDescriptionManager() - rm.load_description('icub') - - with simulated_robot: - pm = ProcessModuleManager.get_manager() - - LookingMotion(Pose([-2, -2, 3])).perform() - time.sleep(2) - MoveTCPMotion(Pose([-0.2, 0.1, 0.1]), Arms.LEFT).perform() - MoveJointsMotion(["torso_roll"], [20.0]).perform() - - pm.exit() - - -class ICUBTestCase(BulletWorldTestCase): - - @classmethod - def setUpClass(cls): - cls.world = BulletWorld(mode=WorldMode.DIRECT) - # cls.milk = Object("milk", pycrap.Milk, "milk.stl", pose=Pose([1.3, 1, 0.9])) - cls.robot = Object("icub", pycrap.Robot, "icub" + cls.extension, pose=Pose([0, 0, 0.5])) - cls.kitchen = Object("kitchen", pycrap.Kitchen, "kitchen" + cls.extension) - # cls.cereal = Object("cereal", pycrap.Cereal, "breakfast_cereal.stl", - # pose=Pose([1.3, 0.7, 0.95])) - ProcessModule.execution_delay = False - ProcessModuleManager.execution_type = ExecutionType.SIMULATED - cls.viz_marker_publisher = VizMarkerPublisher() - - def test_move_joint_direct(self): - self.robot.set_joint_position("l_hand_thumb_0_joint",math.radians(30)) - - - def test_move_joints_simulated(self): - with simulated_robot: - MoveJointsMotion(["torso_roll"], [math.radians(-20.0)]).perform() - sleep(3) - ParkArmsActionPerformable(arm=Arms.BOTH).perform() - sleep(3) - MoveArmJointsMotion(left_arm_poses={"l_elbow": math.radians(20.0)}).perform() - sleep(3) - MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() - sleep(3) - LookingMotion(Pose([-2, -2, 3])).perform() - sleep(3) - MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() - MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() - sleep(20) - MoveGripperMotion(GripperState.CLOSE, Arms.RIGHT).perform() - MoveGripperMotion(GripperState.CLOSE, Arms.LEFT).perform() - sleep(10) - MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() - MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() - - - - - - def test_move_joints(self): - with real_robot: - yarpModule = run_icub_state_updater(['/', '--robot', 'icubSim']) - if yarpModule is None: - print("Failed to open yarp module") - MoveJointsMotion(["torso_roll"], [math.radians(-20.0)]).perform() - - ParkArmsActionPerformable(arm=Arms.BOTH).perform() - - MoveArmJointsMotion(left_arm_poses={"l_elbow": math.radians(20.0)}).perform() - - MoveArmJointsMotion(right_arm_poses={"r_elbow": math.radians(40.0)}).perform() - LookingMotion(Pose([-2, -2, 3])).perform() - MoveGripperMotion(GripperState.OPEN,Arms.LEFT).perform() - sleep(2) - MoveGripperMotion(GripperState.CLOSE,Arms.LEFT).perform() - MoveGripperMotion(GripperState.CLOSE, Arms.RIGHT).perform() - sleep(2) - MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform() - MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() - sleep(2) - - if yarpModule is None: - print("stopping the module") - interrupt_and_close(yarpModule) - - - def test_park_arms(self): - with real_robot: - ParkArmsActionPerformable(arm=Arms.BOTH).perform() - - - def test_try_world(self): - sleep(100) - -# run_look_and_move_tcp() From 9a3b7e34e53d0e6a214789e92d3845c830669e1b Mon Sep 17 00:00:00 2001 From: oeldardear Date: Wed, 11 Dec 2024 10:38:19 +0100 Subject: [PATCH 17/22] [iCub] minor refactoring and documentation --- .../external_interfaces/yarp_networking.py | 34 +++++++++++++- src/pycram/robot_description.py | 11 ++--- .../yarp_utils/yarp_joint_state_updater.py | 44 ++++++++++++++----- 3 files changed, 70 insertions(+), 19 deletions(-) diff --git a/src/pycram/external_interfaces/yarp_networking.py b/src/pycram/external_interfaces/yarp_networking.py index 23ec0ec0e..10437d123 100644 --- a/src/pycram/external_interfaces/yarp_networking.py +++ b/src/pycram/external_interfaces/yarp_networking.py @@ -1,3 +1,4 @@ +from typing_extensions import Tuple,Optional import yarp @@ -7,6 +8,13 @@ def init_yarp_network(): + """ + Initializes the YARP network. + + **Returns:** + - `True` if the YARP network is successfully initialized. + - `False` if no YARP server is found. + """ if not yarp.Network.checkNetwork(): print("Unable to find a yarp server exiting ...") return False @@ -14,7 +22,18 @@ def init_yarp_network(): yarp.Network.init() return True -def open_rpc_client_port(port_name): +def open_rpc_client_port(port_name:str)->Tuple[bool, Optional[yarp.yarp.RpcClient]]: + """ + Opens a YARP RpcClient port with the specified name. + + **Parameters:** + - `port_name` (str): The name of the RPC client port to be opened. + + **Returns:** + - Tuple: (bool, yarp.RpcClient or None) + - `True` and the opened port if successful. + - `False` and `None` if the port fails to open. + """ handle_port: yarp.RpcClient = yarp.RpcClient() if not handle_port.open(port_name): print(f"Can't open the port %s correctly" % port_name) @@ -22,7 +41,18 @@ def open_rpc_client_port(port_name): print(f"Port %s opened correctly" % port_name) return True , handle_port -def open_buffered_bottle_port(port_name): +def open_buffered_bottle_port(port_name:str)->Tuple[bool, Optional[yarp.BufferedPortBottle]]: + """ + Opens a YARP BufferedPortBottle with the specified port name. + + **Parameters:** + - `port_name` (str): The name of the port to be opened. + + **Returns:** + - Tuple: (bool, yarp.BufferedPortBottle or None) + - `True` and the opened port if successful. + - `False` and `None` if the port fails to open. + """ opened_port: yarp.BufferedPortBottle = yarp.BufferedPortBottle() if not opened_port.open(port_name): print(f"Can't open the port %s correctly" % port_name) diff --git a/src/pycram/robot_description.py b/src/pycram/robot_description.py index bcb261f24..305c0b31c 100644 --- a/src/pycram/robot_description.py +++ b/src/pycram/robot_description.py @@ -174,15 +174,12 @@ def get_actuator_for_joint(self, joint: str) -> Optional[Union[str, List[str]]]: return None - def get_actuated_joint_names(self, part_name) -> Optional[str]: + def get_actuated_joint_names(self, part_name:str) -> Optional[str]: """ - Given a part name, returns the actuated joints. + Get the list of the actuated joint names for a given part name. - Args: - part_name (str): part name - - Returns: - List of actuated joint names: or None + :param part_name: part name + return:List of actuated joint names: or None """ return self.joint_actuators.get(part_name) diff --git a/src/pycram/yarp_utils/yarp_joint_state_updater.py b/src/pycram/yarp_utils/yarp_joint_state_updater.py index 92df06acd..3aee180b8 100644 --- a/src/pycram/yarp_utils/yarp_joint_state_updater.py +++ b/src/pycram/yarp_utils/yarp_joint_state_updater.py @@ -1,14 +1,16 @@ import math -from pycram import World -from pycram.failures import YarpNetworkError -from pycram.robot_description import RobotDescription -from pycram.ros.logging import logdebug, logwarn, loginfo, logerr -from pycram.external_interfaces.yarp_networking import * +from ..datastructures.world import World +from ..failures import YarpNetworkError +from ..robot_description import RobotDescription +from ..ros.logging import logdebug, logwarn, loginfo, logerr +from ..external_interfaces.yarp_networking import * class IcubStateUpdater(yarp.RFModule): - + """ + Joint state publisher for the real icub robot update the world + """ def __init__(self): yarp.RFModule.__init__(self) @@ -78,7 +80,7 @@ def configure(self, rf): self.torso_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("torso") self.right_arm_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("right_arm") self.left_arm_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("left_arm") - self.head_joints_names = ["neck_pitch","neck_roll","neck_yaw","eyes_tilt","r_eye_pan_joint","l_eye_pan_joint"] + self.head_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("head") loginfo("Initialization complete") return True @@ -218,7 +220,7 @@ def getPeriod(self): Module refresh rate. Returns : The period of the module in seconds. """ - return 1 + return 0.5 def update_joint_degree(self,joint_name:str,joint_value_degree:float): try: @@ -247,8 +249,8 @@ def update_head_state(self,head_bottle : yarp.Bottle, head_names: [str]): r_eye_pan = 0.5 * head_bottle.get(4).asFloat64() - 0.5 * head_bottle.get(5).asFloat64() l_eye_pan = 0.5 * head_bottle.get(4).asFloat64() + 0.5 * head_bottle.get(5).asFloat64() - self.update_joint_degree(head_names[4],r_eye_pan) - self.update_joint_degree(head_names[5],l_eye_pan) + self.update_joint_degree("r_eye_pan_joint",r_eye_pan) + self.update_joint_degree("l_eye_pan_joint",l_eye_pan) @@ -326,6 +328,13 @@ def update_arm_state(self,arm_bottle : yarp.Bottle, arms_names: [str],arm:str): def updateModule(self): + """ + Periodically called function to update the states of different robotic components. + + This function is invoked at regular intervals determined by the return value of the `getPeriod` function. + It reads sensor data for the torso, right arm, left arm, and head, and processes the data to update their respective states in simulation. + + """ torso_state: yarp.Bottle = self.state_torso_port.read(shouldWait=False) right_arm_state: yarp.Bottle = self.state_right_arm_port.read(shouldWait=False) left_arm_state: yarp.Bottle = self.state_left_arm_port.read(shouldWait=False) @@ -350,6 +359,21 @@ def updateModule(self): def run_icub_state_updater(args): + """ + Initializes and runs the iCub state updater module. + + **Functionality:** + - Verifies if the YARP network is available; raises an error if not. + - Creates an instance of `IcubStateUpdater` and configures it using a `yarp.ResourceFinder`. + - Runs the module in a threaded manner and logs the outcome. + + **Parameters:** + - `args`: Command-line arguments used to configure the module. ex for python calls ['/', '--robot', 'icubSim'] + + **Returns:** + - The instance of `IcubStateUpdater` if the module runs successfully. + - `None` if the module fails to open. + """ if not yarp.Network.checkNetwork(): raise YarpNetworkError() From ec1fdb5fdc6e54fd2a82298b442e950932409ad0 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Wed, 11 Dec 2024 11:23:45 +0100 Subject: [PATCH 18/22] [iCub] fix --- src/pycram/external_interfaces/yarp_networking.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/pycram/external_interfaces/yarp_networking.py b/src/pycram/external_interfaces/yarp_networking.py index 10437d123..90ab7bc01 100644 --- a/src/pycram/external_interfaces/yarp_networking.py +++ b/src/pycram/external_interfaces/yarp_networking.py @@ -22,7 +22,7 @@ def init_yarp_network(): yarp.Network.init() return True -def open_rpc_client_port(port_name:str)->Tuple[bool, Optional[yarp.yarp.RpcClient]]: +def open_rpc_client_port(port_name:str)->Tuple[bool, Optional[yarp.RpcClient]]: """ Opens a YARP RpcClient port with the specified name. From 2a0776100a4262e14d282f9a7fc9c5b057344bc9 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Wed, 11 Dec 2024 16:25:02 +0100 Subject: [PATCH 19/22] [iCub] by pass yarp --- .../process_modules/icub_process_modules.py | 456 ++++++------ .../robot_descriptions/icub_description.py | 1 - .../yarp_utils/yarp_joint_state_updater.py | 669 +++++++++--------- 3 files changed, 566 insertions(+), 560 deletions(-) diff --git a/src/pycram/process_modules/icub_process_modules.py b/src/pycram/process_modules/icub_process_modules.py index 44acef5a0..88b7dee91 100644 --- a/src/pycram/process_modules/icub_process_modules.py +++ b/src/pycram/process_modules/icub_process_modules.py @@ -84,282 +84,284 @@ def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): HAND_CLOSED = [60.0,60.0,0,85.0,20,85,20,85,85] HAND_OPENED = [0 ,0 ,0,0 ,0 ,0 ,0 ,0 ,0] -class iCubNavigationReal(ProcessModule): - """ - The process module to move the robot from one position to another. - """ - def _execute(self, desig: MoveMotion): - loginfo("iCubNavigationReal function. No implementation ATM") +if yarp is not None: + class iCubNavigationReal(ProcessModule): + """ + The process module to move the robot from one position to another. + """ + def _execute(self, desig: MoveMotion): + loginfo("iCubNavigationReal function. No implementation ATM") -class iCubMoveHeadReal(ProcessModule): - """ - This process module moves the head to look at a specific point in the world coordinate frame. - This point can either be a position or an object. + + class iCubMoveHeadReal(ProcessModule): """ + This process module moves the head to look at a specific point in the world coordinate frame. + This point can either be a position or an object. + """ + + def __init__(self,lock,cmd_port:yarp.RpcClient): + super().__init__(lock) + self.cmd_port = cmd_port + + def _execute(self, desig: LookingMotion): + loginfo("iCubMoveHeadReal") + position_target = desig.target.pose.position + if self.cmd_port.getOutputCount(): + + yarp_bottle_msg: yarp.Bottle = yarp.Bottle() + yarp_bottle_reply: yarp.Bottle = yarp.Bottle() + yarp_bottle_msg.addVocab32('l', 'o', 'o', 'k') + yarp_bottle_msg.addVocab32('3', 'D') + + target_loc: yarp.Bottle = yarp_bottle_msg.addList() + target_loc.addFloat32(position_target.x) + target_loc.addFloat32(position_target.y) + target_loc.addFloat32(position_target.z) + self.cmd_port .write(yarp_bottle_msg, yarp_bottle_reply) + reply_vocab = yarp_bottle_reply.get(0).asVocab32() + + if reply_vocab == NO_ACK_VOCAB: + loginfo("Received: NO_ACK") + return False + elif reply_vocab == ACK_VOCAB: + loginfo("Received: ACK") + return True + else: + loginfo("Received: another reply") + return False + - def __init__(self,lock,cmd_port:yarp.RpcClient): - super().__init__(lock) - self.cmd_port = cmd_port - - def _execute(self, desig: LookingMotion): - loginfo("iCubMoveHeadReal") - position_target = desig.target.pose.position - if self.cmd_port.getOutputCount(): - - yarp_bottle_msg: yarp.Bottle = yarp.Bottle() - yarp_bottle_reply: yarp.Bottle = yarp.Bottle() - yarp_bottle_msg.addVocab32('l', 'o', 'o', 'k') - yarp_bottle_msg.addVocab32('3', 'D') - - target_loc: yarp.Bottle = yarp_bottle_msg.addList() - target_loc.addFloat32(position_target.x) - target_loc.addFloat32(position_target.y) - target_loc.addFloat32(position_target.z) - self.cmd_port .write(yarp_bottle_msg, yarp_bottle_reply) - reply_vocab = yarp_bottle_reply.get(0).asVocab32() - - if reply_vocab == NO_ACK_VOCAB: - loginfo("Received: NO_ACK") - return False - elif reply_vocab == ACK_VOCAB: - loginfo("Received: ACK") - return True else: - loginfo("Received: another reply") + logwarn("control port is not connected") return False - else: - logwarn("control port is not connected") - return False + class iCubMoveGripperReal(ProcessModule): + """ + This process module controls the gripper of the robot. They can either be opened or closed. + Furthermore, it can only moved one gripper at a time. + """ + def __init__(self, lock, + state_ports : [yarp.BufferedPortBottle], + ctp_ports: [yarp.RpcClient]): + super().__init__(lock) + self.state_ports = state_ports + self.ctp_ports = ctp_ports + loginfo("iCubMoveGripperReal initialized") + + + def _execute(self, desig: MoveGripperMotion): + gripper = desig.gripper + required_status = desig.motion + if gripper == Arms.RIGHT: + if required_status == GripperState.CLOSE: + update_hand(HAND_CLOSED,self.ctp_ports[0]) + else: + update_hand(HAND_OPENED, self.ctp_ports[0]) + elif gripper == Arms.LEFT: + if required_status == GripperState.CLOSE: + update_hand(HAND_CLOSED, self.ctp_ports[1]) + else: + update_hand(HAND_OPENED, self.ctp_ports[1]) -class iCubMoveGripperReal(ProcessModule): - """ - This process module controls the gripper of the robot. They can either be opened or closed. - Furthermore, it can only moved one gripper at a time. - """ - def __init__(self, lock, - state_ports : [yarp.BufferedPortBottle], - ctp_ports: [yarp.RpcClient]): - super().__init__(lock) - self.state_ports = state_ports - self.ctp_ports = ctp_ports - loginfo("iCubMoveGripperReal initialized") - - - def _execute(self, desig: MoveGripperMotion): - gripper = desig.gripper - required_status = desig.motion - if gripper == Arms.RIGHT: - if required_status == GripperState.CLOSE: - update_hand(HAND_CLOSED,self.ctp_ports[0]) + elif gripper ==Arms.BOTH: + if required_status == GripperState.CLOSE: + update_hand(HAND_CLOSED, self.ctp_ports[0]) + update_hand(HAND_CLOSED, self.ctp_ports[1]) + else: + update_hand(HAND_OPENED, self.ctp_ports[0]) + update_hand(HAND_OPENED, self.ctp_ports[1]) else: - update_hand(HAND_OPENED, self.ctp_ports[0]) + logwarn ("undefined arm") - elif gripper == Arms.LEFT: - if required_status == GripperState.CLOSE: - update_hand(HAND_CLOSED, self.ctp_ports[1]) - else: - update_hand(HAND_OPENED, self.ctp_ports[1]) - elif gripper ==Arms.BOTH: - if required_status == GripperState.CLOSE: - update_hand(HAND_CLOSED, self.ctp_ports[0]) - update_hand(HAND_CLOSED, self.ctp_ports[1]) - else: - update_hand(HAND_OPENED, self.ctp_ports[0]) - update_hand(HAND_OPENED, self.ctp_ports[1]) - else: - logwarn ("undefined arm") + class iCubDetectingReal(ProcessModule): + """ + This process module tries to detect an object with the given type. To be detected the object has to be in + the field of view of the robot. + """ + def _execute(self, desig: DetectingMotion): + loginfo("iCubDetectingReal. No Implementation ATM") -class iCubDetectingReal(ProcessModule): - """ - This process module tries to detect an object with the given type. To be detected the object has to be in - the field of view of the robot. - """ + class iCubMoveTCPReal(ProcessModule): + """ + This process moves the tool center point of either the right or the left arm. + """ - def _execute(self, desig: DetectingMotion): - loginfo("iCubDetectingReal. No Implementation ATM") + def __init__(self, lock, cmd_port: yarp.RpcClient): + super().__init__(lock) + self.cmd_port = cmd_port + + def _execute(self, desig: MoveTCPMotion): + loginfo("iCubMoveTCPReal") + position_target = desig.target.position + if self.cmd_port.getOutputCount(): + + yarp_bottle_msg: yarp.Bottle = yarp.Bottle() + yarp_bottle_reply: yarp.Bottle = yarp.Bottle() + yarp_bottle_msg.addVocab32('t', 'o', 'u', 'c') + + target_loc: yarp.Bottle = yarp_bottle_msg.addList() + target_loc.addFloat32(position_target.x) + target_loc.addFloat32(position_target.y) + target_loc.addFloat32(position_target.z) + + yarp_bottle_msg.addString("side") + if desig.arm == Arms.LEFT: + yarp_bottle_msg.addString("left") + elif desig.arm == Arms.RIGHT: + yarp_bottle_msg.addString("right") + + yarp_bottle_msg.addString("still") + + self.cmd_port.write(yarp_bottle_msg, yarp_bottle_reply) + reply_vocab = yarp_bottle_reply.get(0).asVocab32() + + if reply_vocab == NO_ACK_VOCAB: + loginfo("Received: NO_ACK") + return False + elif reply_vocab == ACK_VOCAB: + loginfo("Received: ACK") + return True + else: + loginfo("Received: another reply") + return False -class iCubMoveTCPReal(ProcessModule): - """ - This process moves the tool center point of either the right or the left arm. - """ - def __init__(self, lock, cmd_port: yarp.RpcClient): - super().__init__(lock) - self.cmd_port = cmd_port + else: + logwarn("port is not connected") + return False - def _execute(self, desig: MoveTCPMotion): - loginfo("iCubMoveTCPReal") - position_target = desig.target.position - if self.cmd_port.getOutputCount(): - yarp_bottle_msg: yarp.Bottle = yarp.Bottle() - yarp_bottle_reply: yarp.Bottle = yarp.Bottle() - yarp_bottle_msg.addVocab32('t', 'o', 'u', 'c') - target_loc: yarp.Bottle = yarp_bottle_msg.addList() - target_loc.addFloat32(position_target.x) - target_loc.addFloat32(position_target.y) - target_loc.addFloat32(position_target.z) - yarp_bottle_msg.addString("side") - if desig.arm == Arms.LEFT: - yarp_bottle_msg.addString("left") - elif desig.arm == Arms.RIGHT: - yarp_bottle_msg.addString("right") + class iCubMoveArmJointsReal(ProcessModule): + """ + This process modules moves the joints of either the right or the left arm. The joint states can be given as + list that should be applied or a pre-defined position can be used, such as "parking" + """ - yarp_bottle_msg.addString("still") + def __init__(self, lock, + state_ports : [yarp.BufferedPortBottle], + ctp_ports: [yarp.RpcClient]): + super().__init__(lock) + self.state_ports = state_ports + self.ctp_ports = ctp_ports + loginfo("iCubMoveArmJointsReal initialized") - self.cmd_port.write(yarp_bottle_msg, yarp_bottle_reply) - reply_vocab = yarp_bottle_reply.get(0).asVocab32() - if reply_vocab == NO_ACK_VOCAB: - loginfo("Received: NO_ACK") - return False - elif reply_vocab == ACK_VOCAB: - loginfo("Received: ACK") - return True - else: - loginfo("Received: another reply") - return False + def _execute(self, desig: MoveArmJointsMotion): + right_arm_to_change_joints = [] + left_arm_to_change_joints = [] - else: - logwarn("port is not connected") - return False + right_arm_to_change_joints_states = [] + left_arm_to_change_joints_states = [] + right_arm_to_change = desig.right_arm_poses + left_arm_to_change = desig.left_arm_poses + if right_arm_to_change is not None: + for joint_mame , joint_pose in right_arm_to_change.items(): + part_name,part_index,joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices(joint_mame) + if part_index is not None: + if part_index == 1: + right_arm_to_change_joints.append(joint_index) + right_arm_to_change_joints_states.append(joint_pose) + else: + logwarn("error in joint name. (Not part of right arm chain)") + else: + logwarn("error in joint name") + if left_arm_to_change is not None: + for joint_mame, joint_pose in left_arm_to_change.items(): + part_name,part_index, joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices( + joint_mame) + if part_index is not None: + if part_index == 2: + left_arm_to_change_joints.append(joint_index) + left_arm_to_change_joints_states.append(joint_pose) + else: + logwarn("error in joint name. (Not part of left arm chain)") + else: + logwarn("error in joint name") -class iCubMoveArmJointsReal(ProcessModule): - """ - This process modules moves the joints of either the right or the left arm. The joint states can be given as - list that should be applied or a pre-defined position can be used, such as "parking" - """ + update_part(self.state_ports[0], + self.ctp_ports[0], + right_arm_to_change_joints, + right_arm_to_change_joints_states) + + update_part(self.state_ports[1], + self.ctp_ports[1], + left_arm_to_change_joints, + left_arm_to_change_joints_states) + + + + class iCubMoveJointsReal(ProcessModule): + """ + Process Module for generic joint movements, is not confined to the arms but can move any joint of the robot + """ - def __init__(self, lock, - state_ports : [yarp.BufferedPortBottle], - ctp_ports: [yarp.RpcClient]): - super().__init__(lock) - self.state_ports = state_ports - self.ctp_ports = ctp_ports - loginfo("iCubMoveArmJointsReal initialized") + def __init__(self, lock, + state_ports : [yarp.BufferedPortBottle], + ctp_ports: [yarp.RpcClient]): + super().__init__(lock) + self.state_ports = state_ports + self.ctp_ports = ctp_ports + loginfo("iCubMoveJointsReal initialized") - def _execute(self, desig: MoveArmJointsMotion): - right_arm_to_change_joints = [] - left_arm_to_change_joints = [] + def _execute(self, desig: MoveJointsMotion): + torso_to_change_joints = [] + right_arm_to_change_joints = [] + left_arm_to_change_joints = [] - right_arm_to_change_joints_states = [] - left_arm_to_change_joints_states = [] + torso_to_change_joints_states = [] + right_arm_to_change_joints_states = [] + left_arm_to_change_joints_states = [] - right_arm_to_change = desig.right_arm_poses - left_arm_to_change = desig.left_arm_poses + to_change_joints = desig.names + to_change_states = desig.positions - if right_arm_to_change is not None: - for joint_mame , joint_pose in right_arm_to_change.items(): + index = 0 + for joint_mame in to_change_joints: part_name,part_index,joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices(joint_mame) if part_index is not None: - if part_index == 1: + if part_index == 0: + torso_to_change_joints.append(joint_index) + torso_to_change_joints_states.append(to_change_states[index]) + elif part_index == 1: right_arm_to_change_joints.append(joint_index) - right_arm_to_change_joints_states.append(joint_pose) - else: - logwarn("error in joint name. (Not part of right arm chain)") - else: - logwarn("error in joint name") - if left_arm_to_change is not None: - for joint_mame, joint_pose in left_arm_to_change.items(): - part_name,part_index, joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices( - joint_mame) - if part_index is not None: - if part_index == 2: + right_arm_to_change_joints_states.append(to_change_states[index]) + elif part_index == 2: left_arm_to_change_joints.append(joint_index) - left_arm_to_change_joints_states.append(joint_pose) + left_arm_to_change_joints_states.append(to_change_states[index]) else: - logwarn("error in joint name. (Not part of left arm chain)") - else: - logwarn("error in joint name") - - - update_part(self.state_ports[0], - self.ctp_ports[0], - right_arm_to_change_joints, - right_arm_to_change_joints_states) - - update_part(self.state_ports[1], - self.ctp_ports[1], - left_arm_to_change_joints, - left_arm_to_change_joints_states) - - - -class iCubMoveJointsReal(ProcessModule): - """ - Process Module for generic joint movements, is not confined to the arms but can move any joint of the robot - """ - - def __init__(self, lock, - state_ports : [yarp.BufferedPortBottle], - ctp_ports: [yarp.RpcClient]): - super().__init__(lock) - self.state_ports = state_ports - self.ctp_ports = ctp_ports - loginfo("iCubMoveJointsReal initialized") - - - - def _execute(self, desig: MoveJointsMotion): - torso_to_change_joints = [] - right_arm_to_change_joints = [] - left_arm_to_change_joints = [] - - torso_to_change_joints_states = [] - right_arm_to_change_joints_states = [] - left_arm_to_change_joints_states = [] - - to_change_joints = desig.names - to_change_states = desig.positions - - index = 0 - for joint_mame in to_change_joints: - part_name,part_index,joint_index = RobotDescription.current_robot_description.get_actuated_joint_indices(joint_mame) - if part_index is not None: - if part_index == 0: - torso_to_change_joints.append(joint_index) - torso_to_change_joints_states.append(to_change_states[index]) - elif part_index == 1: - right_arm_to_change_joints.append(joint_index) - right_arm_to_change_joints_states.append(to_change_states[index]) - elif part_index == 2: - left_arm_to_change_joints.append(joint_index) - left_arm_to_change_joints_states.append(to_change_states[index]) - else: - logwarn("error in index") + logwarn("error in index") - index += 1 + index += 1 - update_part(self.state_ports[0], - self.ctp_ports[0], - torso_to_change_joints, - torso_to_change_joints_states) + update_part(self.state_ports[0], + self.ctp_ports[0], + torso_to_change_joints, + torso_to_change_joints_states) - update_part(self.state_ports[1], - self.ctp_ports[1], - right_arm_to_change_joints, - right_arm_to_change_joints_states) + update_part(self.state_ports[1], + self.ctp_ports[1], + right_arm_to_change_joints, + right_arm_to_change_joints_states) - update_part(self.state_ports[2], - self.ctp_ports[2], - left_arm_to_change_joints, - left_arm_to_change_joints_states) + update_part(self.state_ports[2], + self.ctp_ports[2], + left_arm_to_change_joints, + left_arm_to_change_joints_states) diff --git a/src/pycram/robot_descriptions/icub_description.py b/src/pycram/robot_descriptions/icub_description.py index ecb84e37e..8775e6fe3 100644 --- a/src/pycram/robot_descriptions/icub_description.py +++ b/src/pycram/robot_descriptions/icub_description.py @@ -7,7 +7,6 @@ from ..ros.ros_tools import get_ros_package_path from ..helper import get_robot_mjcf_path -import icub_models filename = get_ros_package_path('icub_model') + '/urdf/model' + '.urdf' diff --git a/src/pycram/yarp_utils/yarp_joint_state_updater.py b/src/pycram/yarp_utils/yarp_joint_state_updater.py index 3aee180b8..690a868bb 100644 --- a/src/pycram/yarp_utils/yarp_joint_state_updater.py +++ b/src/pycram/yarp_utils/yarp_joint_state_updater.py @@ -4,394 +4,399 @@ from ..failures import YarpNetworkError from ..robot_description import RobotDescription from ..ros.logging import logdebug, logwarn, loginfo, logerr -from ..external_interfaces.yarp_networking import * +try: + from ..external_interfaces.yarp_networking import * +except ImportError: + yarp = None + logwarn("no yarp installation found") -class IcubStateUpdater(yarp.RFModule): - """ - Joint state publisher for the real icub robot update the world - """ - def __init__(self): - yarp.RFModule.__init__(self) - - - self.handle_port = yarp.Port() - self.attach(self.handle_port) - - - self.module_name = None - self.robot_name_yarp = None - - self.state_torso_port_name = None - self.state_right_arm_port_name = None - self.state_left_arm_port_name = None - self.state_head_port_name = None - - self.state_torso_port = yarp.BufferedPortBottle() - self.state_right_arm_port = yarp.BufferedPortBottle() - self.state_left_arm_port = yarp.BufferedPortBottle() - self.state_head_port = yarp.BufferedPortBottle() - - self.torso_joints_names = None - self.right_arm_joints_names = None - self.left_arm_joints_names = None - self.head_joints_names = None - - def configure(self, rf): - self.module_name = rf.check("name", - yarp.Value("pycramICubStatePublisher"), - "module name (string)").asString() +if yarp is not None: + class IcubStateUpdater(yarp.RFModule): + """ + Joint state publisher for the real icub robot update the world + """ + def __init__(self): + yarp.RFModule.__init__(self) - self.robot_name_yarp = rf.check("robot", - yarp.Value("icub"), - "module name (string)").asString() - self.state_torso_port_name = self.getName("/state/torso:i") - self.state_right_arm_port_name = self.getName("/state/right_arm:i") - self.state_left_arm_port_name = self.getName("/state/left_arm:i") - self.state_head_port_name = self.getName("/state/head:i") + self.handle_port = yarp.Port() + self.attach(self.handle_port) - # Create handle port to read message - if not self.handle_port.open('/' + self.module_name): - logerr("Can't open the port correctly") - return False + self.module_name = None + self.robot_name_yarp = None - if not self.state_torso_port.open(self.state_torso_port_name): - logerr(f'Can\'t open the port correctly {self.state_torso_port_name}') - return False + self.state_torso_port_name = None + self.state_right_arm_port_name = None + self.state_left_arm_port_name = None + self.state_head_port_name = None - if not self.state_right_arm_port.open(self.state_right_arm_port_name): - logerr(f'Can\'t open the port correctly {self.state_right_arm_port_name}') - return False + self.state_torso_port = yarp.BufferedPortBottle() + self.state_right_arm_port = yarp.BufferedPortBottle() + self.state_left_arm_port = yarp.BufferedPortBottle() + self.state_head_port = yarp.BufferedPortBottle() - if not self.state_left_arm_port.open(self.state_left_arm_port_name): - logerr(f'Can\'t open the port correctly {self.state_left_arm_port_name}') - return False + self.torso_joints_names = None + self.right_arm_joints_names = None + self.left_arm_joints_names = None + self.head_joints_names = None - if not self.state_head_port.open(self.state_head_port_name): - logerr(f'Can\'t open the port correctly {self.state_head_port_name}') - return False - if not self.connect_ports(): - logerr("Error in connecting ports. Make sure that the robot is already up") - return False + def configure(self, rf): + self.module_name = rf.check("name", + yarp.Value("pycramICubStatePublisher"), + "module name (string)").asString() + + self.robot_name_yarp = rf.check("robot", + yarp.Value("icub"), + "module name (string)").asString() + self.state_torso_port_name = self.getName("/state/torso:i") + self.state_right_arm_port_name = self.getName("/state/right_arm:i") + self.state_left_arm_port_name = self.getName("/state/left_arm:i") + self.state_head_port_name = self.getName("/state/head:i") - self.torso_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("torso") - self.right_arm_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("right_arm") - self.left_arm_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("left_arm") - self.head_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("head") - loginfo("Initialization complete") - return True + # Create handle port to read message + if not self.handle_port.open('/' + self.module_name): + logerr("Can't open the port correctly") + return False - def getName(self, name): - """ - Constructs a full YARP-compatible port name for the module. + if not self.state_torso_port.open(self.state_torso_port_name): + logerr(f'Can\'t open the port correctly {self.state_torso_port_name}') + return False - This method appends a given string (`name`) to the module's base name - (`self.module_name`) to construct a complete port name in the YARP - naming convention. The port name is returned as a formatted string - prefixed with a forward slash (`/`), which is the standard for YARP ports. + if not self.state_right_arm_port.open(self.state_right_arm_port_name): + logerr(f'Can\'t open the port correctly {self.state_right_arm_port_name}') + return False - Args: - name (str): The specific name to append to the module's base name. + if not self.state_left_arm_port.open(self.state_left_arm_port_name): + logerr(f'Can\'t open the port correctly {self.state_left_arm_port_name}') + return False - Returns: - str: The fully constructed YARP-compatible port name. - """ - return f'/{self.module_name}{name}' + if not self.state_head_port.open(self.state_head_port_name): + logerr(f'Can\'t open the port correctly {self.state_head_port_name}') + return False + if not self.connect_ports(): + logerr("Error in connecting ports. Make sure that the robot is already up") + return False - def connect_ports(self): - """ - Establishes connections between the robot's state output ports and the module's state input ports. + self.torso_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("torso") + self.right_arm_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("right_arm") + self.left_arm_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("left_arm") + self.head_joints_names = RobotDescription.current_robot_description.get_actuated_joint_names("head") - This method uses the YARP (Yet Another Robot Platform) networking library to connect the robot's - state output ports (torso, right arm, left arm, head) to the corresponding state input ports in - the module. The connections are established using the TCP protocol to ensure reliable communication. + loginfo("Initialization complete") + return True - If any connection fails, an error message is printed to indicate which port could not connect, - and the method returns `False`. If all connections are successfully established, the method - returns `True`. + def getName(self, name): + """ + Constructs a full YARP-compatible port name for the module. - Returns: - bool: `True` if all ports successfully connect, `False` otherwise. - """ - # Torso state port - connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/torso/state:o", self.state_torso_port_name, "tcp") - if not connection_status: - logdebug("state torso port couldn't connect") - return False - - # Right arm state port - connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/right_arm/state:o",self.state_right_arm_port_name, "tcp") - if not connection_status: - logdebug("state right_arm port couldn't connect") - return False - - # Left Arm state port - connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/left_arm/state:o",self.state_left_arm_port_name , "tcp") - if not connection_status: - logdebug("state left_arm port couldn't connect") - return False - - # Head state port - connection_status = yarp.NetworkBase_connect("/" + self.robot_name_yarp + "/head/state:o", - self.state_head_port_name, "tcp") - if not connection_status: - logdebug("state head port couldn't connect") - return False - - return True - - def interruptModule(self): + This method appends a given string (`name`) to the module's base name + (`self.module_name`) to construct a complete port name in the YARP + naming convention. The port name is returned as a formatted string + prefixed with a forward slash (`/`), which is the standard for YARP ports. - """ - Interrupts all communication ports associated with this object. + Args: + name (str): The specific name to append to the module's base name. - This method is used to temporarily halt the operation of the module by - interrupting the communication ports. Interrupting the ports allows the - system to pause ongoing communication or processing without permanently - closing the connections. + Returns: + str: The fully constructed YARP-compatible port name. + """ + return f'/{self.module_name}{name}' - Returns: - bool: `True` if all ports were successfully interrupted. - """ - self.handle_port.interrupt() - self.state_torso_port.interrupt() - self.state_right_arm_port.interrupt() - self.state_left_arm_port.interrupt() - self.state_head_port.interrupt() + def connect_ports(self): + """ + Establishes connections between the robot's state output ports and the module's state input ports. - return True + This method uses the YARP (Yet Another Robot Platform) networking library to connect the robot's + state output ports (torso, right arm, left arm, head) to the corresponding state input ports in + the module. The connections are established using the TCP protocol to ensure reliable communication. + + If any connection fails, an error message is printed to indicate which port could not connect, + and the method returns `False`. If all connections are successfully established, the method + returns `True`. - def close(self): - """ - Closes all the communication ports associated with this object. + Returns: + bool: `True` if all ports successfully connect, `False` otherwise. + """ + # Torso state port + connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/torso/state:o", self.state_torso_port_name, "tcp") + if not connection_status: + logdebug("state torso port couldn't connect") + return False + + # Right arm state port + connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/right_arm/state:o",self.state_right_arm_port_name, "tcp") + if not connection_status: + logdebug("state right_arm port couldn't connect") + return False + + # Left Arm state port + connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/left_arm/state:o",self.state_left_arm_port_name , "tcp") + if not connection_status: + logdebug("state left_arm port couldn't connect") + return False - This method is responsible for ensuring that all the communication ports - used by the system are properly closed to free up resources and maintain - system integrity. The ports being closed include: + # Head state port + connection_status = yarp.NetworkBase_connect("/" + self.robot_name_yarp + "/head/state:o", + self.state_head_port_name, "tcp") + if not connection_status: + logdebug("state head port couldn't connect") + return False + + return True - - `handle_port`: The port used for handling general rpc commands for the yarp module. - - `state_torso_port`: The port used for monitoring the torso's state. - - `state_right_arm_port`: The port used for monitoring the right arm's state. - - `state_left_arm_port`: The port used for monitoring the left arm's state. - - `state_head_port`: The port used for monitoring the head's state. + def interruptModule(self): - After closing all the ports, the function returns `True` to indicate - successful closure of all resources. + """ + Interrupts all communication ports associated with this object. - Returns: - bool: `True` if all ports were successfully closed. - """ + This method is used to temporarily halt the operation of the module by + interrupting the communication ports. Interrupting the ports allows the + system to pause ongoing communication or processing without permanently + closing the connections. - self.handle_port.close() - self.state_torso_port.close() - self.state_right_arm_port.close() - self.state_left_arm_port.close() - self.state_head_port.close() + Returns: + bool: `True` if all ports were successfully interrupted. + """ - return True + self.handle_port.interrupt() + self.state_torso_port.interrupt() + self.state_right_arm_port.interrupt() + self.state_left_arm_port.interrupt() + self.state_head_port.interrupt() - def respond(self, command, reply): + return True + + def close(self): + """ + Closes all the communication ports associated with this object. + + This method is responsible for ensuring that all the communication ports + used by the system are properly closed to free up resources and maintain + system integrity. The ports being closed include: - """ - Processes a command and generates an appropriate response. + - `handle_port`: The port used for handling general rpc commands for the yarp module. + - `state_torso_port`: The port used for monitoring the torso's state. + - `state_right_arm_port`: The port used for monitoring the right arm's state. + - `state_left_arm_port`: The port used for monitoring the left arm's state. + - `state_head_port`: The port used for monitoring the head's state. - This method handles incoming commands (RPC) through the port /module_name - Further it execute a behaviour and then replies - """ - reply.clear() + After closing all the ports, the function returns `True` to indicate + successful closure of all resources. + + Returns: + bool: `True` if all ports were successfully closed. + """ + + self.handle_port.close() + self.state_torso_port.close() + self.state_right_arm_port.close() + self.state_left_arm_port.close() + self.state_head_port.close() + + return True + + def respond(self, command, reply): + + """ + Processes a command and generates an appropriate response. + + This method handles incoming commands (RPC) through the port /module_name + Further it execute a behaviour and then replies + """ + reply.clear() + + if command.get(0).asString() == "quit": + reply.addString("quitting") + return False + else: + reply.addString("other request") + + return True + + + def getPeriod(self): + """ + Module refresh rate. + Returns : The period of the module in seconds. + """ + return 0.5 + + def update_joint_degree(self,joint_name:str,joint_value_degree:float): + try: + World.robot.set_joint_position(joint_name,math.radians(joint_value_degree)) + + except Exception as e: + logerr(f"error in updating joint {joint_name} to {joint_value_degree} degree") + pass + + + + def update_torso_state(self, part_bottle : yarp.Bottle, part_names: [str]): + if part_bottle.size() == len(part_names): + for i in range(part_bottle.size()): + self.update_joint_degree(part_names[i], + part_bottle.get(i).asFloat64()) + else : + logwarn(f"mismatch in torso joint sizes {part_bottle.size()} , {len(part_names)}") + + def update_head_state(self,head_bottle : yarp.Bottle, head_names: [str]): + for i in range(4): + self.update_joint_degree(head_names[i], + head_bottle.get(i).asFloat64()) + + + r_eye_pan = 0.5 * head_bottle.get(4).asFloat64() - 0.5 * head_bottle.get(5).asFloat64() + l_eye_pan = 0.5 * head_bottle.get(4).asFloat64() + 0.5 * head_bottle.get(5).asFloat64() + + self.update_joint_degree("r_eye_pan_joint",r_eye_pan) + self.update_joint_degree("l_eye_pan_joint",l_eye_pan) + + + + def update_arm_state(self,arm_bottle : yarp.Bottle, arms_names: [str],arm:str): + for i in range(7): + self.update_joint_degree(arms_names[i],arm_bottle.get(i).asFloat64()) + #################################################################################### + #################################################################################### + # hand (7) = index 0 + ring 0 + little 0 + self.update_joint_degree(f"{arm}_hand_index_0_joint", + arm_bottle.get(7).asFloat64()/-3.0) + self.update_joint_degree(f"{arm}_hand_middle_0_joint", + 0) + self.update_joint_degree(f"{arm}_hand_ring_0_joint", + 20 - (arm_bottle.get(7).asFloat64()/3.0)) + self.update_joint_degree(f"{arm}_hand_little_0_joint", + 20 - (arm_bottle.get(7).asFloat64()/3.0)) - if command.get(0).asString() == "quit": - reply.addString("quitting") - return False - else: - reply.addString("other request") + #################################################################################### + # thumb oppose (8) = 0 - return True + self.update_joint_degree(f"{arm}_hand_thumb_0_joint", + arm_bottle.get(8).asFloat64()) + # # thumb proximal (9) = 1 + self.update_joint_degree(f"{arm}_hand_thumb_1_joint", + arm_bottle.get(9).asFloat64()) - def getPeriod(self): - """ - Module refresh rate. - Returns : The period of the module in seconds. - """ - return 0.5 - - def update_joint_degree(self,joint_name:str,joint_value_degree:float): - try: - World.robot.set_joint_position(joint_name,math.radians(joint_value_degree)) - - except Exception as e: - logerr(f"error in updating joint {joint_name} to {joint_value_degree} degree") - pass - - - - def update_torso_state(self, part_bottle : yarp.Bottle, part_names: [str]): - if part_bottle.size() == len(part_names): - for i in range(part_bottle.size()): - self.update_joint_degree(part_names[i], - part_bottle.get(i).asFloat64()) - else : - logwarn(f"mismatch in torso joint sizes {part_bottle.size()} , {len(part_names)}") - - def update_head_state(self,head_bottle : yarp.Bottle, head_names: [str]): - for i in range(4): - self.update_joint_degree(head_names[i], - head_bottle.get(i).asFloat64()) - - - r_eye_pan = 0.5 * head_bottle.get(4).asFloat64() - 0.5 * head_bottle.get(5).asFloat64() - l_eye_pan = 0.5 * head_bottle.get(4).asFloat64() + 0.5 * head_bottle.get(5).asFloat64() - - self.update_joint_degree("r_eye_pan_joint",r_eye_pan) - self.update_joint_degree("l_eye_pan_joint",l_eye_pan) - - - - def update_arm_state(self,arm_bottle : yarp.Bottle, arms_names: [str],arm:str): - for i in range(7): - self.update_joint_degree(arms_names[i],arm_bottle.get(i).asFloat64()) - #################################################################################### - #################################################################################### - # hand (7) = index 0 + ring 0 + little 0 - self.update_joint_degree(f"{arm}_hand_index_0_joint", - arm_bottle.get(7).asFloat64()/-3.0) - self.update_joint_degree(f"{arm}_hand_middle_0_joint", - 0) - self.update_joint_degree(f"{arm}_hand_ring_0_joint", - 20 - (arm_bottle.get(7).asFloat64()/3.0)) - self.update_joint_degree(f"{arm}_hand_little_0_joint", - 20 - (arm_bottle.get(7).asFloat64()/3.0)) - - #################################################################################### - # thumb oppose (8) = 0 - - self.update_joint_degree(f"{arm}_hand_thumb_0_joint", - arm_bottle.get(8).asFloat64()) - - # # thumb proximal (9) = 1 - self.update_joint_degree(f"{arm}_hand_thumb_1_joint", - arm_bottle.get(9).asFloat64()) - - # thumb distal (10) = 2 + 3 - self.update_joint_degree(f"{arm}_hand_thumb_2_joint", - 0.5*arm_bottle.get(10).asFloat64()) - self.update_joint_degree(f"{arm}_hand_thumb_3_joint", - 0.5*arm_bottle.get(10).asFloat64()) - #################################################################################### - # # index proximal (11) = 1 - self.update_joint_degree(f"{arm}_hand_index_1_joint", - arm_bottle.get(11).asFloat64()) - - # index distal (12) = 2 + 3 - self.update_joint_degree(f"{arm}_hand_index_2_joint", - 0.5 * arm_bottle.get(12).asFloat64()) - self.update_joint_degree(f"{arm}_hand_index_3_joint", - 0.5 * arm_bottle.get(12).asFloat64()) - #################################################################################### - # middle proximal (13) = 1 - self.update_joint_degree(f"{arm}_hand_middle_1_joint", - arm_bottle.get(13).asFloat64()) - - # middle distal (14) = 2 + 3 - self.update_joint_degree(f"{arm}_hand_middle_2_joint", - 0.5 * arm_bottle.get(14).asFloat64()) - self.update_joint_degree(f"{arm}_hand_middle_3_joint", - 0.5 * arm_bottle.get(14).asFloat64()) - #################################################################################### - - # Pinky (15) = 1 + 2 + 3 (little) (ring) - self.update_joint_degree(f"{arm}_hand_ring_1_joint", - arm_bottle.get(15).asFloat64()/3.0) - self.update_joint_degree(f"{arm}_hand_ring_2_joint", - arm_bottle.get(15).asFloat64()/3.0) - self.update_joint_degree(f"{arm}_hand_ring_3_joint", - arm_bottle.get(15).asFloat64()/3.0) - #################################################################################### - # Pinky (15) = 1 + 2 + 3 (little) (ring) - self.update_joint_degree(f"{arm}_hand_little_1_joint", - arm_bottle.get(15).asFloat64()/3.0) - self.update_joint_degree(f"{arm}_hand_little_2_joint", - arm_bottle.get(15).asFloat64()/3.0) - self.update_joint_degree(f"{arm}_hand_little_3_joint", - arm_bottle.get(15).asFloat64()/3.0) - #################################################################################### - - - - - - def updateModule(self): - """ - Periodically called function to update the states of different robotic components. + # thumb distal (10) = 2 + 3 + self.update_joint_degree(f"{arm}_hand_thumb_2_joint", + 0.5*arm_bottle.get(10).asFloat64()) + self.update_joint_degree(f"{arm}_hand_thumb_3_joint", + 0.5*arm_bottle.get(10).asFloat64()) + #################################################################################### + # # index proximal (11) = 1 + self.update_joint_degree(f"{arm}_hand_index_1_joint", + arm_bottle.get(11).asFloat64()) - This function is invoked at regular intervals determined by the return value of the `getPeriod` function. - It reads sensor data for the torso, right arm, left arm, and head, and processes the data to update their respective states in simulation. + # index distal (12) = 2 + 3 + self.update_joint_degree(f"{arm}_hand_index_2_joint", + 0.5 * arm_bottle.get(12).asFloat64()) + self.update_joint_degree(f"{arm}_hand_index_3_joint", + 0.5 * arm_bottle.get(12).asFloat64()) + #################################################################################### + # middle proximal (13) = 1 + self.update_joint_degree(f"{arm}_hand_middle_1_joint", + arm_bottle.get(13).asFloat64()) - """ - torso_state: yarp.Bottle = self.state_torso_port.read(shouldWait=False) - right_arm_state: yarp.Bottle = self.state_right_arm_port.read(shouldWait=False) - left_arm_state: yarp.Bottle = self.state_left_arm_port.read(shouldWait=False) - head_state: yarp.Bottle = self.state_head_port.read(shouldWait=False) + # middle distal (14) = 2 + 3 + self.update_joint_degree(f"{arm}_hand_middle_2_joint", + 0.5 * arm_bottle.get(14).asFloat64()) + self.update_joint_degree(f"{arm}_hand_middle_3_joint", + 0.5 * arm_bottle.get(14).asFloat64()) + #################################################################################### - if torso_state is not None: - self.update_torso_state(torso_state, self.torso_joints_names) + # Pinky (15) = 1 + 2 + 3 (little) (ring) + self.update_joint_degree(f"{arm}_hand_ring_1_joint", + arm_bottle.get(15).asFloat64()/3.0) + self.update_joint_degree(f"{arm}_hand_ring_2_joint", + arm_bottle.get(15).asFloat64()/3.0) + self.update_joint_degree(f"{arm}_hand_ring_3_joint", + arm_bottle.get(15).asFloat64()/3.0) + #################################################################################### + # Pinky (15) = 1 + 2 + 3 (little) (ring) + self.update_joint_degree(f"{arm}_hand_little_1_joint", + arm_bottle.get(15).asFloat64()/3.0) + self.update_joint_degree(f"{arm}_hand_little_2_joint", + arm_bottle.get(15).asFloat64()/3.0) + self.update_joint_degree(f"{arm}_hand_little_3_joint", + arm_bottle.get(15).asFloat64()/3.0) + #################################################################################### - if right_arm_state is not None: - self.update_arm_state(right_arm_state,self.right_arm_joints_names,"r") - if left_arm_state is not None: - self.update_arm_state(left_arm_state,self.left_arm_joints_names,"l") - if head_state is not None: - self.update_head_state(head_state,self.head_joints_names) - return True + def updateModule(self): + """ + Periodically called function to update the states of different robotic components. + This function is invoked at regular intervals determined by the return value of the `getPeriod` function. + It reads sensor data for the torso, right arm, left arm, and head, and processes the data to update their respective states in simulation. + """ + torso_state: yarp.Bottle = self.state_torso_port.read(shouldWait=False) + right_arm_state: yarp.Bottle = self.state_right_arm_port.read(shouldWait=False) + left_arm_state: yarp.Bottle = self.state_left_arm_port.read(shouldWait=False) + head_state: yarp.Bottle = self.state_head_port.read(shouldWait=False) + if torso_state is not None: + self.update_torso_state(torso_state, self.torso_joints_names) -def run_icub_state_updater(args): - """ - Initializes and runs the iCub state updater module. + if right_arm_state is not None: + self.update_arm_state(right_arm_state,self.right_arm_joints_names,"r") - **Functionality:** - - Verifies if the YARP network is available; raises an error if not. - - Creates an instance of `IcubStateUpdater` and configures it using a `yarp.ResourceFinder`. - - Runs the module in a threaded manner and logs the outcome. + if left_arm_state is not None: + self.update_arm_state(left_arm_state,self.left_arm_joints_names,"l") - **Parameters:** - - `args`: Command-line arguments used to configure the module. ex for python calls ['/', '--robot', 'icubSim'] + if head_state is not None: + self.update_head_state(head_state,self.head_joints_names) - **Returns:** - - The instance of `IcubStateUpdater` if the module runs successfully. - - `None` if the module fails to open. + return True + + + + + + def run_icub_state_updater(args): """ - if not yarp.Network.checkNetwork(): - raise YarpNetworkError() - - m_module = IcubStateUpdater() - - rf = yarp.ResourceFinder() - rf.setVerbose(True) - rf.setDefaultContext('pycram') - #rf.setDefaultConfigFile('icub_joint_ipdater.ini') - - if rf.configure(args): - logdebug("icub_state_updater Configuration done") - if m_module.runModuleThreaded(rf) == 0: - loginfo("done running module") - return m_module - else: - interrupt_and_close(m_module) - logerr("Module Failed to open") - return None + Initializes and runs the iCub state updater module. + + **Functionality:** + - Verifies if the YARP network is available; raises an error if not. + - Creates an instance of `IcubStateUpdater` and configures it using a `yarp.ResourceFinder`. + - Runs the module in a threaded manner and logs the outcome. + + **Parameters:** + - `args`: Command-line arguments used to configure the module. ex for python calls ['/', '--robot', 'icubSim'] + + **Returns:** + - The instance of `IcubStateUpdater` if the module runs successfully. + - `None` if the module fails to open. + """ + if not yarp.Network.checkNetwork(): + raise YarpNetworkError() + + m_module = IcubStateUpdater() + + rf = yarp.ResourceFinder() + rf.setVerbose(True) + rf.setDefaultContext('pycram') + #rf.setDefaultConfigFile('icub_joint_ipdater.ini') + + if rf.configure(args): + logdebug("icub_state_updater Configuration done") + if m_module.runModuleThreaded(rf) == 0: + loginfo("done running module") + return m_module + else: + interrupt_and_close(m_module) + logerr("Module Failed to open") + return None From 3da3b3377708bd11dff228af42d9dc2064fb1d15 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Wed, 11 Dec 2024 17:20:46 +0100 Subject: [PATCH 20/22] [iCub] minor structure and documentation changes --- .../external_interfaces/yarp_networking.py | 102 +++++++++--------- .../process_modules/icub_process_modules.py | 98 +++++++++-------- .../yarp_utils/yarp_joint_state_updater.py | 31 ++---- 3 files changed, 112 insertions(+), 119 deletions(-) diff --git a/src/pycram/external_interfaces/yarp_networking.py b/src/pycram/external_interfaces/yarp_networking.py index 90ab7bc01..18551e78d 100644 --- a/src/pycram/external_interfaces/yarp_networking.py +++ b/src/pycram/external_interfaces/yarp_networking.py @@ -1,66 +1,66 @@ from typing_extensions import Tuple,Optional -import yarp +from pycram.ros.logging import logwarn +try: + import yarp +except ImportError: + logwarn("yarp wasn't found, please check the installation") -ACK_VOCAB = yarp.createVocab32('a','c','k') -NO_ACK_VOCAB = yarp.createVocab32('n','a','c','k') +if yarp is not None: + ACK_VOCAB = yarp.createVocab32('a','c','k') + NO_ACK_VOCAB = yarp.createVocab32('n','a','c','k') -def init_yarp_network(): - """ - Initializes the YARP network. - **Returns:** - - `True` if the YARP network is successfully initialized. - - `False` if no YARP server is found. - """ - if not yarp.Network.checkNetwork(): - print("Unable to find a yarp server exiting ...") - return False + def init_yarp_network(): + """ + Initializes the YARP network. - yarp.Network.init() - return True + :param func: Function that should be thread safe + :return bool -> true if the YARP network is successfully initialized. + """ + if not yarp.Network.checkNetwork(): + print("Unable to find a yarp server exiting ...") + return False -def open_rpc_client_port(port_name:str)->Tuple[bool, Optional[yarp.RpcClient]]: - """ - Opens a YARP RpcClient port with the specified name. + yarp.Network.init() + return True - **Parameters:** - - `port_name` (str): The name of the RPC client port to be opened. + def open_rpc_client_port(port_name:str)->Tuple[bool, Optional[yarp.RpcClient]]: + """ + Opens a YARP RpcClient port with the specified name. - **Returns:** - - Tuple: (bool, yarp.RpcClient or None) - - `True` and the opened port if successful. - - `False` and `None` if the port fails to open. - """ - handle_port: yarp.RpcClient = yarp.RpcClient() - if not handle_port.open(port_name): - print(f"Can't open the port %s correctly" % port_name) - return False , None - print(f"Port %s opened correctly" % port_name) - return True , handle_port + :param `port_name` (str): The name of the RPC client port to be opened. + :return Tuple: (bool ( success/ fail ), yarp.RpcClient or None) + """ + handle_port: yarp.RpcClient = yarp.RpcClient() + if not handle_port.open(port_name): + print(f"Can't open the port %s correctly" % port_name) + return False , None + print(f"Port %s opened correctly" % port_name) + return True , handle_port -def open_buffered_bottle_port(port_name:str)->Tuple[bool, Optional[yarp.BufferedPortBottle]]: - """ - Opens a YARP BufferedPortBottle with the specified port name. + def open_buffered_bottle_port(port_name:str)->Tuple[bool, Optional[yarp.BufferedPortBottle]]: + """ + Opens a YARP BufferedPortBottle with the specified port name. - **Parameters:** - - `port_name` (str): The name of the port to be opened. + :param `port_name` (str): The name of the port to be opened. + :return Tuple: (bool ( success/ fail ), yarp.BufferedPortBottle or None) + """ + opened_port: yarp.BufferedPortBottle = yarp.BufferedPortBottle() + if not opened_port.open(port_name): + print(f"Can't open the port %s correctly" % port_name) + return False , None + print(f"Port %s opened correctly" % port_name) + return True , opened_port - **Returns:** - - Tuple: (bool, yarp.BufferedPortBottle or None) - - `True` and the opened port if successful. - - `False` and `None` if the port fails to open. - """ - opened_port: yarp.BufferedPortBottle = yarp.BufferedPortBottle() - if not opened_port.open(port_name): - print(f"Can't open the port %s correctly" % port_name) - return False , None - print(f"Port %s opened correctly" % port_name) - return True , opened_port - -def interrupt_and_close(m_module:yarp.RFModule): - m_module.interruptModule() - m_module.close() + def interrupt_and_close(m_module:yarp.RFModule): + """ + interrupt and close the module + :param m_module: yarp module + """ + m_module.interruptModule() + m_module.close() + print("iCub state updater closed") diff --git a/src/pycram/process_modules/icub_process_modules.py b/src/pycram/process_modules/icub_process_modules.py index 88b7dee91..7a4421360 100644 --- a/src/pycram/process_modules/icub_process_modules.py +++ b/src/pycram/process_modules/icub_process_modules.py @@ -20,34 +20,18 @@ yarp = None pass -def update_hand(hand_values,ctp_port): - yarp_bottle_msg: yarp.Bottle = yarp.Bottle() - yarp_bottle_reply: yarp.Bottle = yarp.Bottle() - yarp_bottle_msg.addVocab32('c', 't', 'p', 'q') - yarp_bottle_msg.addVocab32('t', 'i', 'm', 'e') - yarp_bottle_msg.addFloat32(1.5) - yarp_bottle_msg.addVocab32('o', 'f', 'f') - yarp_bottle_msg.addInt32(7) - yarp_bottle_msg.addVocab32('p', 'o', 's') - target_loc: yarp.Bottle = yarp_bottle_msg.addList() - for i in hand_values: - target_loc.addFloat32(i) - - - ctp_port.write(yarp_bottle_msg, yarp_bottle_reply) - reply_vocab = yarp_bottle_reply.get(0).asVocab32() - - if reply_vocab == NO_ACK_VOCAB: - loginfo("Received: NO_ACK") - return False - elif reply_vocab == ACK_VOCAB: - loginfo("Received: ACK") - return True - else: - loginfo("Received: another reply") - return False + def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): + """ + constant time position control of specified part on a robotic part using YARP communication. + :param state_port: a status port to get the current states of the joints for the whole part + :param ctp_port:the control port + :param joint_to_change_idx: a list of joint names to change + :param joints_to_change_pos: a list of joint positions + + :return: bool. success/fail for the control action + """ if len(joint_to_change_idx): part_state: yarp.Bottle = state_port.read(shouldWait=True) part_new_states = [] @@ -82,8 +66,7 @@ def update_part(state_port,ctp_port, joint_to_change_idx, joints_to_change_pos): loginfo("Received: another reply") return False -HAND_CLOSED = [60.0,60.0,0,85.0,20,85,20,85,85] -HAND_OPENED = [0 ,0 ,0,0 ,0 ,0 ,0 ,0 ,0] + if yarp is not None: class iCubNavigationReal(ProcessModule): @@ -101,7 +84,7 @@ class iCubMoveHeadReal(ProcessModule): This point can either be a position or an object. """ - def __init__(self,lock,cmd_port:yarp.RpcClient): + def __init__(self,lock,cmd_port:"yarp.RpcClient"): super().__init__(lock) self.cmd_port = cmd_port @@ -145,36 +128,63 @@ class iCubMoveGripperReal(ProcessModule): Furthermore, it can only moved one gripper at a time. """ def __init__(self, lock, - state_ports : [yarp.BufferedPortBottle], - ctp_ports: [yarp.RpcClient]): + state_ports : ["yarp.BufferedPortBottle"], + ctp_ports: ["yarp.RpcClient"]): super().__init__(lock) self.state_ports = state_ports self.ctp_ports = ctp_ports + self.HAND_CLOSED = [60.0,60.0,0,85.0,20,85,20,85,85] + self.HAND_OPENED = [0 ,0 ,0,0 ,0 ,0 ,0 ,0 ,0] loginfo("iCubMoveGripperReal initialized") + def update_hand(self,hand_values, ctp_port): + yarp_bottle_msg: yarp.Bottle = yarp.Bottle() + yarp_bottle_reply: yarp.Bottle = yarp.Bottle() + yarp_bottle_msg.addVocab32('c', 't', 'p', 'q') + yarp_bottle_msg.addVocab32('t', 'i', 'm', 'e') + yarp_bottle_msg.addFloat32(1.5) + yarp_bottle_msg.addVocab32('o', 'f', 'f') + yarp_bottle_msg.addInt32(7) + yarp_bottle_msg.addVocab32('p', 'o', 's') + target_loc: yarp.Bottle = yarp_bottle_msg.addList() + for i in hand_values: + target_loc.addFloat32(i) + + ctp_port.write(yarp_bottle_msg, yarp_bottle_reply) + reply_vocab = yarp_bottle_reply.get(0).asVocab32() + + if reply_vocab == NO_ACK_VOCAB: + loginfo("Received: NO_ACK") + return False + elif reply_vocab == ACK_VOCAB: + loginfo("Received: ACK") + return True + else: + loginfo("Received: another reply") + return False def _execute(self, desig: MoveGripperMotion): gripper = desig.gripper required_status = desig.motion if gripper == Arms.RIGHT: if required_status == GripperState.CLOSE: - update_hand(HAND_CLOSED,self.ctp_ports[0]) + self.update_hand(self.HAND_CLOSED,self.ctp_ports[0]) else: - update_hand(HAND_OPENED, self.ctp_ports[0]) + self.update_hand(self.HAND_OPENED, self.ctp_ports[0]) elif gripper == Arms.LEFT: if required_status == GripperState.CLOSE: - update_hand(HAND_CLOSED, self.ctp_ports[1]) + self.update_hand(self.HAND_CLOSED, self.ctp_ports[1]) else: - update_hand(HAND_OPENED, self.ctp_ports[1]) + self.update_hand(self.HAND_OPENED, self.ctp_ports[1]) elif gripper ==Arms.BOTH: if required_status == GripperState.CLOSE: - update_hand(HAND_CLOSED, self.ctp_ports[0]) - update_hand(HAND_CLOSED, self.ctp_ports[1]) + self.update_hand(self.HAND_CLOSED, self.ctp_ports[0]) + self.update_hand(self.HAND_CLOSED, self.ctp_ports[1]) else: - update_hand(HAND_OPENED, self.ctp_ports[0]) - update_hand(HAND_OPENED, self.ctp_ports[1]) + self.update_hand(self.HAND_OPENED, self.ctp_ports[0]) + self.update_hand(self.HAND_OPENED, self.ctp_ports[1]) else: logwarn ("undefined arm") @@ -193,7 +203,7 @@ class iCubMoveTCPReal(ProcessModule): This process moves the tool center point of either the right or the left arm. """ - def __init__(self, lock, cmd_port: yarp.RpcClient): + def __init__(self, lock, cmd_port: "yarp.RpcClient"): super().__init__(lock) self.cmd_port = cmd_port @@ -247,8 +257,8 @@ class iCubMoveArmJointsReal(ProcessModule): """ def __init__(self, lock, - state_ports : [yarp.BufferedPortBottle], - ctp_ports: [yarp.RpcClient]): + state_ports : ["yarp.BufferedPortBottle"], + ctp_ports: ["yarp.RpcClient"]): super().__init__(lock) self.state_ports = state_ports self.ctp_ports = ctp_ports @@ -309,8 +319,8 @@ class iCubMoveJointsReal(ProcessModule): """ def __init__(self, lock, - state_ports : [yarp.BufferedPortBottle], - ctp_ports: [yarp.RpcClient]): + state_ports : ["yarp.BufferedPortBottle"], + ctp_ports: ["yarp.RpcClient"]): super().__init__(lock) self.state_ports = state_ports self.ctp_ports = ctp_ports diff --git a/src/pycram/yarp_utils/yarp_joint_state_updater.py b/src/pycram/yarp_utils/yarp_joint_state_updater.py index 690a868bb..80849b9d2 100644 --- a/src/pycram/yarp_utils/yarp_joint_state_updater.py +++ b/src/pycram/yarp_utils/yarp_joint_state_updater.py @@ -6,7 +6,6 @@ from ..ros.logging import logdebug, logwarn, loginfo, logerr try: from ..external_interfaces.yarp_networking import * - except ImportError: yarp = None logwarn("no yarp installation found") @@ -99,11 +98,8 @@ def getName(self, name): naming convention. The port name is returned as a formatted string prefixed with a forward slash (`/`), which is the standard for YARP ports. - Args: - name (str): The specific name to append to the module's base name. - - Returns: - str: The fully constructed YARP-compatible port name. + :param: name (str): The specific name to append to the module's base name. + :return: The fully constructed YARP-compatible port name. """ return f'/{self.module_name}{name}' @@ -120,8 +116,7 @@ def connect_ports(self): and the method returns `False`. If all connections are successfully established, the method returns `True`. - Returns: - bool: `True` if all ports successfully connect, `False` otherwise. + :return bool: `True` if all ports successfully connect, `False` otherwise. """ # Torso state port connection_status = yarp.NetworkBase_connect("/"+self.robot_name_yarp+"/torso/state:o", self.state_torso_port_name, "tcp") @@ -160,8 +155,7 @@ def interruptModule(self): system to pause ongoing communication or processing without permanently closing the connections. - Returns: - bool: `True` if all ports were successfully interrupted. + :return bool: `True` if all ports were successfully interrupted. """ self.handle_port.interrupt() @@ -180,17 +174,10 @@ def close(self): used by the system are properly closed to free up resources and maintain system integrity. The ports being closed include: - - `handle_port`: The port used for handling general rpc commands for the yarp module. - - `state_torso_port`: The port used for monitoring the torso's state. - - `state_right_arm_port`: The port used for monitoring the right arm's state. - - `state_left_arm_port`: The port used for monitoring the left arm's state. - - `state_head_port`: The port used for monitoring the head's state. - After closing all the ports, the function returns `True` to indicate successful closure of all resources. - Returns: - bool: `True` if all ports were successfully closed. + :returns `True` if all ports were successfully closed or 'false' """ self.handle_port.close() @@ -367,17 +354,13 @@ def run_icub_state_updater(args): """ Initializes and runs the iCub state updater module. - **Functionality:** - Verifies if the YARP network is available; raises an error if not. - Creates an instance of `IcubStateUpdater` and configures it using a `yarp.ResourceFinder`. - Runs the module in a threaded manner and logs the outcome. - **Parameters:** - - `args`: Command-line arguments used to configure the module. ex for python calls ['/', '--robot', 'icubSim'] + :param: Command-line arguments used to configure the module. ex for python calls ['/', '--robot', 'icubSim'] - **Returns:** - - The instance of `IcubStateUpdater` if the module runs successfully. - - `None` if the module fails to open. + :return: The instance of `IcubStateUpdater` if the module runs successfully.`None` if the module fails to open. """ if not yarp.Network.checkNetwork(): raise YarpNetworkError() From 949efc06dc5e7dfd40a23e2d11d480fbfa1f1bc8 Mon Sep 17 00:00:00 2001 From: oeldardear Date: Wed, 11 Dec 2024 17:42:47 +0100 Subject: [PATCH 21/22] [iCub] minor fix --- src/pycram/process_modules/icub_process_modules.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/pycram/process_modules/icub_process_modules.py b/src/pycram/process_modules/icub_process_modules.py index 7a4421360..91e95b6da 100644 --- a/src/pycram/process_modules/icub_process_modules.py +++ b/src/pycram/process_modules/icub_process_modules.py @@ -559,7 +559,7 @@ def move_joints(self): elif ProcessModuleManager.execution_type == ExecutionType.REAL: if not self.initialized: - raise RobotNotInitialized() + raise RobotNotInitialized("icub") return iCubMoveJointsReal(self._move_joints_lock, [self.state_torso_port,self.state_right_arm_port,self.state_left_arm_port], [self.ctp_torso_client_port,self.ctp_right_arm_client_port,self.ctp_left_arm_client_port]) @@ -569,7 +569,7 @@ def move_gripper(self): return iCubMoveGripper(self._move_gripper_lock) elif ProcessModuleManager.execution_type == ExecutionType.REAL: if not self.initialized: - raise RobotNotInitialized() + raise RobotNotInitialized("icub") return iCubMoveGripperReal(self._move_gripper_lock, [self.state_right_arm_port, self.state_left_arm_port], [self.ctp_right_arm_client_port,self.ctp_left_arm_client_port]) From c78f0263b0c7d8f2a07efa3df4778d8f44947dae Mon Sep 17 00:00:00 2001 From: oeldardear Date: Thu, 12 Dec 2024 11:46:15 +0100 Subject: [PATCH 22/22] add icub demo --- demos/icub_real_demo.ipynb | 672 +++++++++++++++++++++++++++++++++++++ 1 file changed, 672 insertions(+) create mode 100644 demos/icub_real_demo.ipynb diff --git a/demos/icub_real_demo.ipynb b/demos/icub_real_demo.ipynb new file mode 100644 index 000000000..ae8f0cb38 --- /dev/null +++ b/demos/icub_real_demo.ipynb @@ -0,0 +1,672 @@ +{ + "cells": [ + { + "cell_type": "code", + "id": "initial_id", + "metadata": { + "collapsed": true, + "ExecuteTime": { + "end_time": "2024-12-12T10:21:06.235605Z", + "start_time": "2024-12-12T10:20:36.259912Z" + } + }, + "source": [ + "import math\n", + "\n", + "import pycrap\n", + "from pycram.ros.logging import loginfo, logerr\n", + "from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher\n", + "from pycram.worlds.bullet_world import BulletWorld\n", + "from pycram.designators.action_designator import *\n", + "from pycram.designators.location_designator import *\n", + "from pycram.datastructures.enums import WorldMode , GripperState\n", + "from pycram.datastructures.pose import Pose\n", + "from pycram.world_concepts.world_object import Object\n", + "from pycram.yarp_utils.yarp_joint_state_updater import run_icub_state_updater\n", + "from pycram.external_interfaces.yarp_networking import interrupt_and_close\n", + "from pycram.process_module import ProcessModuleManager, ProcessModule, real_robot\n" + ], + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[WARN] [1733998847.585535]: [giskard.py:26:] Failed to import Giskard messages, the real robot will not be available\n", + "[WARN] [1733998850.618016]: [robokudo.py:21:] Failed to import Robokudo messages, the real robot will not be available\n", + "[WARN] [1733998852.808435]: [move_base.py:13:] Could not import MoveBase messages, Navigation interface could not be initialized\n", + "pycram_bullet build time: Sep 5 2024 08:54:44\n", + "/usr/local/src/robot/Bremen/envs/pycram/lib/python3.8/site-packages/pydub/utils.py:170: RuntimeWarning: Couldn't find ffmpeg or avconv - defaulting to ffmpeg, but may not work\n", + " warn(\"Couldn't find ffmpeg or avconv - defaulting to ffmpeg, but may not work\", RuntimeWarning)\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='base_transmission']/actuator[@name='base_r_drive_wheel_joint_actuator']\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='base_transmission']/actuator[@name='base_l_drive_wheel_joint_actuator']\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='base_roll_joint_transmission']/actuator[@name='base_roll_joint_actuator']\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='head_pan_joint_transmission']/actuator[@name='head_pan_joint_actuator']\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='head_tilt_joint_transmission']/actuator[@name='head_tilt_joint_actuator']\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='arm_lift_joint_transmission']/actuator[@name='arm_lift_joint_actuator']\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='arm_flex_joint_transmission']/actuator[@name='arm_flex_joint_actuator']\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='arm_roll_joint_transmission']/actuator[@name='arm_roll_joint_actuator']\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='wrist_flex_joint_transmission']/actuator[@name='wrist_flex_joint_actuator']\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='wrist_roll_joint_transmission']/actuator[@name='wrist_roll_joint_actuator']\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='hand_motor_joint_transmission']/actuator[@name='hand_motor_joint_actuator']\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='hand_l_spring_proximal_joint_transmission']/actuator[@name='hand_l_spring_proximal_joint_actuator']\n", + "Unknown tag \"hardwareInterface\" in /robot[@name='hsrb']/transmission[@name='hand_r_spring_proximal_joint_transmission']/actuator[@name='hand_r_spring_proximal_joint_actuator']\n", + "[WARN] [1733998857.625821]: [ur5e_controlled_description.py:17:] Could not initialize ur5e description as Multiverse resources path not found.\n", + "Unknown tag \"motorTorqueConstant\" in /robot[@name='iai_donbot']/transmission[@name='gripper_base_trans_left']\n", + "Unknown tag \"contact_coefficients\" in /robot[@name='iai_donbot']/link[@name='gripper_gripper_left_link']\n", + "Unknown tag \"contact_coefficients\" in /robot[@name='iai_donbot']/link[@name='gripper_finger_left_link']\n", + "Unknown tag \"motorTorqueConstant\" in /robot[@name='iai_donbot']/transmission[@name='gripper_base_trans_right']\n", + "Unknown tag \"contact_coefficients\" in /robot[@name='iai_donbot']/link[@name='gripper_gripper_right_link']\n", + "Unknown tag \"contact_coefficients\" in /robot[@name='iai_donbot']/link[@name='gripper_finger_right_link']\n", + "Unknown tag \"material\" in /robot[@name='iai_donbot']/link[@name='camera_holder_link']/collision[1]\n", + "Unknown tag \"material\" in /robot[@name='iai_donbot']/link[@name='switches']/collision[1]\n", + "Unknown tag \"material\" in /robot[@name='iai_donbot']/link[@name='charger']/collision[1]\n", + "Unknown tag \"material\" in /robot[@name='iai_donbot']/link[@name='e_stop']/collision[1]\n", + "Unknown tag \"material\" in /robot[@name='iai_donbot']/link[@name='wlan']/collision[1]\n", + "Unknown tag \"material\" in /robot[@name='iai_donbot']/link[@name='ur5_touchpad']/collision[1]\n", + "Unknown tag \"material\" in /robot[@name='iai_donbot']/link[@name='wrist_collision']/collision[1]\n", + "Unknown attribute \"spring_reference\" in /robot[@name='stretch_description']/joint[@name='joint_right_wheel']/dynamics\n", + "Unknown attribute \"spring_stiffness\" in /robot[@name='stretch_description']/joint[@name='joint_right_wheel']/dynamics\n", + "Unknown attribute \"spring_reference\" in /robot[@name='stretch_description']/joint[@name='joint_left_wheel']/dynamics\n", + "Unknown attribute \"spring_stiffness\" in /robot[@name='stretch_description']/joint[@name='joint_left_wheel']/dynamics\n", + "Unknown tag \"surface\" in /robot[@name='stretch_description']/link[@name='caster_link']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_thumb_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_thumb_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_thumb_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_thumb_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_index_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_index_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_index_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_index_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_middle_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_middle_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_middle_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_middle_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_ring_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_ring_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_ring_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_ring_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_little_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_little_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_little_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_little_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_thumb_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_thumb_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_thumb_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_thumb_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_index_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_index_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_index_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_index_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_middle_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_middle_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_middle_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_ring_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_ring_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_ring_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_ring_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_little_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_little_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_little_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_little_3']/collision[1]\n", + "Unknown tag \"sensor\" in /robot[@name='iCub']\n", + "[WARN] [1733998860.746190]: [helper.py:73:get_robot_mjcf_path] Multiverse resources path not found.\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='base_laser_link']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='wide_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='narrow_stereo_optical_frame']\n", + "Unknown attribute \"type\" in /robot[@name='pr2']/link[@name='laser_tilt_link']\n", + "[WARN] [1733998863.975821]: [helper.py:73:get_robot_mjcf_path] Multiverse resources path not found.\n", + "Unknown attribute \"type\" in /robot[@name='tiago_dual']/link[@name='base_laser_link']\n", + "Unknown tag \"material\" in /robot[@name='tiago_dual']/link[@name='base_link']/collision[1]\n", + "Unknown tag \"material\" in /robot[@name='tiago_dual']/link[@name='base_antenna_left_link']/collision[1]\n", + "Unknown tag \"material\" in /robot[@name='tiago_dual']/link[@name='base_antenna_right_link']/collision[1]\n", + "Unknown tag \"kinematic\" in /robot[@name='tiago_dual']/link[@name='suspension_right_link']\n", + "Unknown tag \"gravity\" in /robot[@name='tiago_dual']/link[@name='suspension_right_link']\n", + "Unknown tag \"kinematic\" in /robot[@name='tiago_dual']/link[@name='suspension_left_link']\n", + "Unknown tag \"gravity\" in /robot[@name='tiago_dual']/link[@name='suspension_left_link']\n", + "Unknown tag \"material\" in /robot[@name='tiago_dual']/link[@name='torso_fixed_column_link']/collision[1]\n", + "Unknown tag \"material\" in /robot[@name='tiago_dual']/link[@name='torso_lift_link']/collision[1]\n", + "Unknown tag \"material\" in /robot[@name='tiago_dual']/link[@name='torso_lift_link']/collision[2]\n", + "Unknown tag \"material\" in /robot[@name='tiago_dual']/link[@name='torso_lift_link']/collision[3]\n", + "Unknown tag \"material\" in /robot[@name='tiago_dual']/link[@name='torso_lift_link']/collision[4]\n", + "Unknown tag \"material\" in /robot[@name='tiago_dual']/link[@name='head_1_link']/collision[1]\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='head_1_trans']/joint[@name='head_1_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='head_2_trans']/joint[@name='head_2_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_5_trans']/joint[@name='arm_left_5_joint']\n", + "Unknown tag \"role\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_wrist_trans']/actuator[@name='arm_left_6_motor']\n", + "Unknown tag \"role\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_wrist_trans']/actuator[@name='arm_left_7_motor']\n", + "Unknown tag \"role\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_wrist_trans']/joint[@name='arm_left_6_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_wrist_trans']/joint[@name='arm_left_6_joint']\n", + "Unknown tag \"mechanicalReduction\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_wrist_trans']/joint[@name='arm_left_6_joint']\n", + "Unknown tag \"ignoreTransmissionAbsoluteEncoder\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_wrist_trans']/joint[@name='arm_left_6_joint']\n", + "Unknown tag \"role\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_wrist_trans']/joint[@name='arm_left_7_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_wrist_trans']/joint[@name='arm_left_7_joint']\n", + "Unknown tag \"mechanicalReduction\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_wrist_trans']/joint[@name='arm_left_7_joint']\n", + "Unknown tag \"ignoreTransmissionAbsoluteEncoder\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_wrist_trans']/joint[@name='arm_left_7_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_1_trans']/joint[@name='arm_left_1_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_2_trans']/joint[@name='arm_left_2_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_3_trans']/joint[@name='arm_left_3_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_left_4_trans']/joint[@name='arm_left_4_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_5_trans']/joint[@name='arm_right_5_joint']\n", + "Unknown tag \"role\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_wrist_trans']/actuator[@name='arm_right_6_motor']\n", + "Unknown tag \"role\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_wrist_trans']/actuator[@name='arm_right_7_motor']\n", + "Unknown tag \"role\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_wrist_trans']/joint[@name='arm_right_6_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_wrist_trans']/joint[@name='arm_right_6_joint']\n", + "Unknown tag \"mechanicalReduction\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_wrist_trans']/joint[@name='arm_right_6_joint']\n", + "Unknown tag \"ignoreTransmissionAbsoluteEncoder\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_wrist_trans']/joint[@name='arm_right_6_joint']\n", + "Unknown tag \"role\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_wrist_trans']/joint[@name='arm_right_7_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_wrist_trans']/joint[@name='arm_right_7_joint']\n", + "Unknown tag \"mechanicalReduction\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_wrist_trans']/joint[@name='arm_right_7_joint']\n", + "Unknown tag \"ignoreTransmissionAbsoluteEncoder\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_wrist_trans']/joint[@name='arm_right_7_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_1_trans']/joint[@name='arm_right_1_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_2_trans']/joint[@name='arm_right_2_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_3_trans']/joint[@name='arm_right_3_joint']\n", + "Unknown tag \"offset\" in /robot[@name='tiago_dual']/transmission[@name='arm_right_4_trans']/joint[@name='arm_right_4_joint']\n", + "Unknown tag \"motorTorqueConstant\" in /robot[@name='boxy']/transmission[@name='left_gripper_base_trans_left']\n", + "Unknown tag \"contact_coefficients\" in /robot[@name='boxy']/link[@name='left_gripper_gripper_left_link']\n", + "Unknown tag \"contact_coefficients\" in /robot[@name='boxy']/link[@name='left_gripper_finger_left_link']\n", + "Unknown tag \"motorTorqueConstant\" in /robot[@name='boxy']/transmission[@name='left_gripper_base_trans_right']\n", + "Unknown tag \"contact_coefficients\" in /robot[@name='boxy']/link[@name='left_gripper_gripper_right_link']\n", + "Unknown tag \"contact_coefficients\" in /robot[@name='boxy']/link[@name='left_gripper_finger_right_link']\n", + "Unknown tag \"motorTorqueConstant\" in /robot[@name='boxy']/transmission[@name='right_gripper_base_trans_left']\n", + "Unknown tag \"contact_coefficients\" in /robot[@name='boxy']/link[@name='right_gripper_gripper_left_link']\n", + "Unknown tag \"contact_coefficients\" in /robot[@name='boxy']/link[@name='right_gripper_finger_left_link']\n", + "Unknown tag \"motorTorqueConstant\" in /robot[@name='boxy']/transmission[@name='right_gripper_base_trans_right']\n", + "Unknown tag \"contact_coefficients\" in /robot[@name='boxy']/link[@name='right_gripper_gripper_right_link']\n", + "Unknown tag \"contact_coefficients\" in /robot[@name='boxy']/link[@name='right_gripper_finger_right_link']\n", + "Unknown attribute \"iyx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_base_link']/inertial/inertia\n", + "Unknown attribute \"izx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_base_link']/inertial/inertia\n", + "Unknown attribute \"izy\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_base_link']/inertial/inertia\n", + "Unknown attribute \"iyx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_knuckle_link']/inertial/inertia\n", + "Unknown attribute \"izx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_knuckle_link']/inertial/inertia\n", + "Unknown attribute \"izy\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_knuckle_link']/inertial/inertia\n", + "Unknown attribute \"iyx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_knuckle_link']/inertial/inertia\n", + "Unknown attribute \"izx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_knuckle_link']/inertial/inertia\n", + "Unknown attribute \"izy\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_knuckle_link']/inertial/inertia\n", + "Unknown attribute \"iyx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_finger_link']/inertial/inertia\n", + "Unknown attribute \"izx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_finger_link']/inertial/inertia\n", + "Unknown attribute \"izy\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_finger_link']/inertial/inertia\n", + "Unknown attribute \"iyx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_finger_link']/inertial/inertia\n", + "Unknown attribute \"izx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_finger_link']/inertial/inertia\n", + "Unknown attribute \"izy\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_finger_link']/inertial/inertia\n", + "Unknown attribute \"iyx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_inner_knuckle_link']/inertial/inertia\n", + "Unknown attribute \"izx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_inner_knuckle_link']/inertial/inertia\n", + "Unknown attribute \"izy\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_inner_knuckle_link']/inertial/inertia\n", + "Unknown attribute \"iyx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_inner_knuckle_link']/inertial/inertia\n", + "Unknown attribute \"izx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_inner_knuckle_link']/inertial/inertia\n", + "Unknown attribute \"izy\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_inner_knuckle_link']/inertial/inertia\n", + "Unknown attribute \"iyx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_finger_tip_link']/inertial/inertia\n", + "Unknown attribute \"izx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_finger_tip_link']/inertial/inertia\n", + "Unknown attribute \"izy\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_finger_tip_link']/inertial/inertia\n", + "Unknown tag \"contact\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_left_finger_tip_link']\n", + "Unknown attribute \"iyx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_finger_tip_link']/inertial/inertia\n", + "Unknown attribute \"izx\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_finger_tip_link']/inertial/inertia\n", + "Unknown attribute \"izy\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_finger_tip_link']/inertial/inertia\n", + "Unknown tag \"contact\" in /robot[@name='ur5_robotiq']/link[@name='robotiq_85_right_finger_tip_link']\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[INFO] [1733998866.034292]: [icub_process_modules.py:500:__init__] yarp network state detected\n", + "[INFO] |yarp.os.Port|/pycram/gaze/cmd:oi| Port /pycram/gaze/cmd:oi active at tcp://134.102.113.35:10241/\n", + "Port /pycram/gaze/cmd:oi opened correctly\n", + "[INFO] |yarp.os.Port|/pycram/action/cmd:oi| Port /pycram/action/cmd:oi active at tcp://134.102.113.35:10242/\n", + "Port /pycram/action/cmd:oi opened correctly\n", + "[INFO] |yarp.os.Port|/pycram/ctp/torso:oi| Port /pycram/ctp/torso:oi active at tcp://134.102.113.35:10243/\n", + "Port /pycram/ctp/torso:oi opened correctly\n", + "Port /pycram/ctp/right_arm:oi opened correctly\n", + "[INFO] |yarp.os.Port|/pycram/ctp/right_arm:oi| Port /pycram/ctp/right_arm:oi active at tcp://134.102.113.35:10244/\n", + "[INFO] |yarp.os.Port|/pycram/ctp/left_arm:oi| Port /pycram/ctp/left_arm:oi active at tcp://134.102.113.35:10245/\n", + "Port /pycram/ctp/left_arm:oi opened correctly\n", + "[INFO] |yarp.os.Port|/pycram/state/torso:i| Port /pycram/state/torso:i active at tcp://134.102.113.35:10246/\n", + "Port /pycram/state/torso:i opened correctly\n", + "Port /pycram/state/right_arm:i opened correctly\n", + "[INFO] |yarp.os.Port|/pycram/state/right_arm:i| Port /pycram/state/right_arm:i active at tcp://134.102.113.35:10247/\n", + "Port /pycram/state/left_arm:i opened correctly\n", + "[INFO] |yarp.os.Port|/pycram/state/left_arm:i| Port /pycram/state/left_arm:i active at tcp://134.102.113.35:10248/\n", + "[INFO] |yarp.os.impl.PortCoreOutputUnit|/pycram/gaze/cmd:oi| Sending output from /pycram/gaze/cmd:oi to /iKinGazeCtrl/rpc using tcp\n", + "[INFO] |yarp.os.impl.PortCoreOutputUnit|/pycram/action/cmd:oi| Sending output from /pycram/action/cmd:oi to /actionsRenderingEngine/cmd:io using tcp\n", + "[INFO] |yarp.os.impl.PortCoreOutputUnit|/pycram/ctp/torso:oi| Sending output from /pycram/ctp/torso:oi to /ctpservice/torso/rpc using tcp\n", + "[INFO] |yarp.os.impl.PortCoreOutputUnit|/pycram/ctp/right_arm:oi| Sending output from /pycram/ctp/right_arm:oi to /ctpservice/right_arm/rpc using tcp\n", + "[INFO] |yarp.os.impl.PortCoreOutputUnit|/pycram/ctp/left_arm:oi| Sending output from /pycram/ctp/left_arm:oi to /ctpservice/left_arm/rpc using tcp\n", + "[INFO] |yarp.os.impl.PortCoreInputUnit|/pycram/state/torso:i| Receiving input from /icubSim/torso/state:o to /pycram/state/torso:i using tcp\n", + "[INFO] |yarp.os.impl.PortCoreInputUnit|/pycram/state/right_arm:i| Receiving input from /icubSim/right_arm/state:o to /pycram/state/right_arm:i using tcp\n", + "[INFO] |yarp.os.impl.PortCoreInputUnit|/pycram/state/left_arm:i| Receiving input from /icubSim/left_arm/state:o to /pycram/state/left_arm:i using tcp\n" + ] + } + ], + "execution_count": 1 + }, + { + "metadata": {}, + "cell_type": "markdown", + "source": "Initializing a world", + "id": "b023ed641c9930a2" + }, + { + "metadata": { + "ExecuteTime": { + "end_time": "2024-12-12T10:21:07.179121Z", + "start_time": "2024-12-12T10:21:06.604972Z" + } + }, + "cell_type": "code", + "source": [ + "world = BulletWorld(mode=WorldMode.DIRECT)\n", + "viz_marker_publisher = VizMarkerPublisher()" + ], + "id": "7d124114c826647c", + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[INFO] [1733998867.162122]: [cache_manager.py:102:look_for_file_in_data_dir] Found file plane.urdf in /usr/local/src/robot/Bremen/workspace/ros/src/pycram/resources/objects/plane.urdf\n" + ] + } + ], + "execution_count": 2 + }, + { + "metadata": {}, + "cell_type": "markdown", + "source": "adding the robot and the environment", + "id": "453ea6e790fa13b9" + }, + { + "metadata": { + "ExecuteTime": { + "end_time": "2024-12-12T10:21:10.918788Z", + "start_time": "2024-12-12T10:21:07.233772Z" + } + }, + "cell_type": "code", + "source": [ + "robot = Object(\"icub\", pycrap.Robot, \"icub.urdf\" , pose=Pose([0, 0, 0.6]))\n", + "kitchen = Object(\"kitchen\", pycrap.Kitchen, \"kitchen.urdf\" )" + ], + "id": "d833f6103b1948", + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[INFO] [1733998867.335881]: [cache_manager.py:102:look_for_file_in_data_dir] Found file icub.urdf in /usr/local/src/robot/Bremen/workspace/ros/src/pycram/resources/robots/icub.urdf\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_thumb_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_thumb_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_thumb_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_thumb_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_index_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_index_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_index_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_index_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_middle_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_middle_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_middle_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_middle_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_ring_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_ring_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_ring_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_ring_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_little_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_little_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_little_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='r_hand_little_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_thumb_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_thumb_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_thumb_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_thumb_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_index_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_index_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_index_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_index_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_middle_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_middle_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_middle_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_ring_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_ring_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_ring_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_ring_3']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_little_0']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_little_1']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_little_2']/collision[1]\n", + "Unknown tag \"max_contacts\" in /robot[@name='iCub']/link[@name='l_hand_little_3']/collision[1]\n", + "Unknown tag \"sensor\" in /robot[@name='iCub']\n" + ] + }, + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[INFO] [1733998870.166128]: [cache_manager.py:102:look_for_file_in_data_dir] Found file kitchen.urdf in /usr/local/src/robot/Bremen/workspace/ros/src/pycram/resources/objects/kitchen.urdf\n" + ] + }, + { + "name": "stderr", + "output_type": "stream", + "text": [ + "Scalar element defined multiple times: limit\n" + ] + } + ], + "execution_count": 3 + }, + { + "metadata": {}, + "cell_type": "markdown", + "source": "if ypu want the status update for icub run the following cell", + "id": "dccfd7af9bd28ded" + }, + { + "metadata": { + "ExecuteTime": { + "end_time": "2024-12-12T10:21:13.087337Z", + "start_time": "2024-12-12T10:21:10.934705Z" + } + }, + "cell_type": "code", + "source": [ + "yarpModule = run_icub_state_updater(['/', '--robot', 'icubSim'])\n", + "if yarpModule is None:\n", + " print(\"Failed to open yarp module\")" + ], + "id": "7630c6f0f95b5d26", + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[DEBUG] |yarp.os.ResourceFinder| clearing context\n", + "[DEBUG] |yarp.os.ResourceFinder| adding context [pycram]\n", + "[DEBUG] |yarp.os.ResourceFinder| configuring\n", + "[DEBUG] |yarp.os.ResourceFinder| finding paths [plugins]\n", + "[INFO] |yarp.os.Port|/pycramICubStatePublisher| Port /pycramICubStatePublisher active at tcp://134.102.113.35:10249/\n", + "[INFO] |yarp.os.Port|/pycramICubStatePublisher/state/torso:i| Port /pycramICubStatePublisher/state/torso:i active at tcp://134.102.113.35:10250/\n", + "[INFO] |yarp.os.Port|/pycramICubStatePublisher/state/right_arm:i| Port /pycramICubStatePublisher/state/right_arm:i active at tcp://134.102.113.35:10251/\n", + "[INFO] |yarp.os.Port|/pycramICubStatePublisher/state/left_arm:i| Port /pycramICubStatePublisher/state/left_arm:i active at tcp://134.102.113.35:10252/\n", + "[INFO] |yarp.os.Port|/pycramICubStatePublisher/state/head:i| Port /pycramICubStatePublisher/state/head:i active at tcp://134.102.113.35:10253/\n", + "[INFO] |yarp.os.impl.PortCoreInputUnit|/pycramICubStatePublisher/state/torso:i| Receiving input from /icubSim/torso/state:o to /pycramICubStatePublisher/state/torso:i using tcp\n", + "[INFO] |yarp.os.impl.PortCoreInputUnit|/pycramICubStatePublisher/state/right_arm:i| Receiving input from /icubSim/right_arm/state:o to /pycramICubStatePublisher/state/right_arm:i using tcp\n", + "[INFO] |yarp.os.impl.PortCoreInputUnit|/pycramICubStatePublisher/state/left_arm:i| Receiving input from /icubSim/left_arm/state:o to /pycramICubStatePublisher/state/left_arm:i using tcp\n", + "[INFO] |yarp.os.impl.PortCoreInputUnit|/pycramICubStatePublisher/state/head:i| Receiving input from /icubSim/head/state:o to /pycramICubStatePublisher/state/head:i using tcp\n", + "[INFO] [1733998871.543331]: [yarp_joint_state_updater.py:89:configure] Initialization complete\n", + "[INFO] [1733998872.974025]: [yarp_joint_state_updater.py:378:run_icub_state_updater] done running module\n" + ] + } + ], + "execution_count": 4 + }, + { + "metadata": {}, + "cell_type": "markdown", + "source": "some motion action examples", + "id": "b318bf76b13335e2" + }, + { + "metadata": { + "ExecuteTime": { + "end_time": "2024-12-12T10:21:16.721756Z", + "start_time": "2024-12-12T10:21:13.643172Z" + } + }, + "cell_type": "code", + "source": [ + "with real_robot:\n", + " MoveJointsMotion([\"torso_roll\"], [math.radians(-20.0)]).perform()" + ], + "id": "379f2a349c197bf3", + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[INFO] [1733998875.212483]: [icub_process_modules.py:327:__init__] iCubMoveJointsReal initialized\n", + "[INFO] [1733998876.597550]: [icub_process_modules.py:63:update_part] Received: ACK\n" + ] + } + ], + "execution_count": 5 + }, + { + "metadata": { + "ExecuteTime": { + "end_time": "2024-12-12T10:21:20.678885Z", + "start_time": "2024-12-12T10:21:17.199857Z" + } + }, + "cell_type": "code", + "source": [ + "with real_robot:\n", + " ParkArmsActionPerformable(arm=Arms.BOTH).perform()" + ], + "id": "718536502a94753c", + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[INFO] [1733998878.800949]: [icub_process_modules.py:265:__init__] iCubMoveArmJointsReal initialized\n", + "[INFO] [1733998879.588164]: [icub_process_modules.py:63:update_part] Received: ACK\n", + "[INFO] [1733998880.630365]: [icub_process_modules.py:63:update_part] Received: ACK\n" + ] + } + ], + "execution_count": 6 + }, + { + "metadata": { + "ExecuteTime": { + "end_time": "2024-12-12T10:21:24.898744Z", + "start_time": "2024-12-12T10:21:21.181701Z" + } + }, + "cell_type": "code", + "source": [ + "with real_robot:\n", + " MoveArmJointsMotion(left_arm_poses={\"l_elbow\": math.radians(20.0)}).perform()" + ], + "id": "5d34fb06e0b8ffd8", + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[INFO] [1733998883.031935]: [icub_process_modules.py:265:__init__] iCubMoveArmJointsReal initialized\n", + "[INFO] [1733998884.762697]: [icub_process_modules.py:63:update_part] Received: ACK\n" + ] + } + ], + "execution_count": 7 + }, + { + "metadata": { + "ExecuteTime": { + "end_time": "2024-12-12T10:21:29.143019Z", + "start_time": "2024-12-12T10:21:25.446144Z" + } + }, + "cell_type": "code", + "source": [ + "with real_robot:\n", + " MoveArmJointsMotion(right_arm_poses={\"r_elbow\": math.radians(40.0)}).perform()" + ], + "id": "7693dc3f9b5f96ef", + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[INFO] [1733998887.445865]: [icub_process_modules.py:265:__init__] iCubMoveArmJointsReal initialized\n", + "[INFO] [1733998888.996222]: [icub_process_modules.py:63:update_part] Received: ACK\n" + ] + } + ], + "execution_count": 8 + }, + { + "metadata": { + "ExecuteTime": { + "end_time": "2024-12-12T10:21:33.203066Z", + "start_time": "2024-12-12T10:21:29.692034Z" + } + }, + "cell_type": "code", + "source": [ + "with real_robot:\n", + " LookingMotion(Pose([-2, -2, 3])).perform()" + ], + "id": "1a94c6e30cecd78", + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[INFO] [1733998891.506742]: [icub_process_modules.py:92:_execute] iCubMoveHeadReal\n", + "[INFO] [1733998893.077215]: [icub_process_modules.py:112:_execute] Received: ACK\n" + ] + } + ], + "execution_count": 9 + }, + { + "metadata": { + "ExecuteTime": { + "end_time": "2024-12-12T10:21:40.125488Z", + "start_time": "2024-12-12T10:21:33.666043Z" + } + }, + "cell_type": "code", + "source": [ + "with real_robot:\n", + " MoveGripperMotion(GripperState.CLOSE, Arms.RIGHT).perform()\n", + " MoveGripperMotion(GripperState.CLOSE, Arms.LEFT).perform()" + ], + "id": "252c072b9beb1180", + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[INFO] [1733998894.887572]: [icub_process_modules.py:138:__init__] iCubMoveGripperReal initialized\n", + "[INFO] [1733998896.695699]: [icub_process_modules.py:160:update_hand] Received: ACK\n", + "[INFO] [1733998898.377648]: [icub_process_modules.py:138:__init__] iCubMoveGripperReal initialized\n", + "[INFO] [1733998900.007856]: [icub_process_modules.py:160:update_hand] Received: ACK\n" + ] + } + ], + "execution_count": 10 + }, + { + "metadata": { + "ExecuteTime": { + "end_time": "2024-12-12T10:21:47.279059Z", + "start_time": "2024-12-12T10:21:40.637119Z" + } + }, + "cell_type": "code", + "source": [ + "with real_robot:\n", + " MoveGripperMotion(GripperState.OPEN, Arms.RIGHT).perform()\n", + " MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform()" + ], + "id": "fc03037631b39bea", + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[INFO] [1733998902.183713]: [icub_process_modules.py:138:__init__] iCubMoveGripperReal initialized\n", + "[INFO] [1733998903.877550]: [icub_process_modules.py:160:update_hand] Received: ACK\n", + "[INFO] [1733998905.499243]: [icub_process_modules.py:138:__init__] iCubMoveGripperReal initialized\n", + "[INFO] [1733998907.107124]: [icub_process_modules.py:160:update_hand] Received: ACK\n" + ] + } + ], + "execution_count": 11 + }, + { + "metadata": { + "ExecuteTime": { + "end_time": "2024-12-12T10:21:49.594102Z", + "start_time": "2024-12-12T10:21:47.995741Z" + } + }, + "cell_type": "code", + "source": [ + "if yarpModule is not None:\n", + " loginfo(\"stopping the module\")\n", + " interrupt_and_close(yarpModule)\n" + ], + "id": "55ab5f06e4d0e0cb", + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "[INFO] [1733998909.369400]: [2392790548.py:2:] stopping the module\n", + "[INFO] |yarp.os.impl.PortCoreInputUnit|/pycramICubStatePublisher/state/torso:i| Removing input from /icubSim/torso/state:o to /pycramICubStatePublisher/state/torso:i\n", + "[INFO] |yarp.os.impl.PortCoreInputUnit|/pycramICubStatePublisher/state/right_arm:i| Removing input from /icubSim/right_arm/state:o to /pycramICubStatePublisher/state/right_arm:i\n", + "[INFO] |yarp.os.impl.PortCoreInputUnit|/pycramICubStatePublisher/state/left_arm:i| Removing input from /icubSim/left_arm/state:o to /pycramICubStatePublisher/state/left_arm:i\n", + "[INFO] |yarp.os.impl.PortCoreInputUnit|/pycramICubStatePublisher/state/head:i| Removing input from /icubSim/head/state:o to /pycramICubStatePublisher/state/head:i\n", + "iCub state updater closed\n" + ] + } + ], + "execution_count": 12 + }, + { + "metadata": { + "ExecuteTime": { + "end_time": "2024-12-12T10:21:50.823758Z", + "start_time": "2024-12-12T10:21:50.100245Z" + } + }, + "cell_type": "code", + "source": [ + "#viz_marker_publisher._stop_publishing()\n", + "world.exit()" + ], + "id": "cb2c354ab424d955", + "outputs": [ + { + "name": "stderr", + "output_type": "stream", + "text": [ + "[ERROR] [1733998910.532109]: [yarp_joint_state_updater.py:222:update_joint_degree] error in updating joint r_hand_index_3_joint to 0.0 degree\n", + "[ERROR] [1733998910.810581]: [yarp_joint_state_updater.py:222:update_joint_degree] error in updating joint r_hand_middle_1_joint to 0.0 degree\n", + "[ERROR] [1733998910.814906]: [yarp_joint_state_updater.py:222:update_joint_degree] error in updating joint r_hand_middle_2_joint to 0.0 degree\n" + ] + } + ], + "execution_count": 13 + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 2 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython2", + "version": "2.7.6" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +}