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 +} diff --git a/launch/ik_and_description.launch b/launch/ik_and_description.launch index 52565a7fa..24c8d7441 100644 --- a/launch/ik_and_description.launch +++ b/launch/ik_and_description.launch @@ -16,6 +16,12 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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/external_interfaces/yarp_networking.py b/src/pycram/external_interfaces/yarp_networking.py new file mode 100644 index 000000000..18551e78d --- /dev/null +++ b/src/pycram/external_interfaces/yarp_networking.py @@ -0,0 +1,66 @@ +from typing_extensions import Tuple,Optional + +from pycram.ros.logging import logwarn + +try: + import yarp +except ImportError: + logwarn("yarp wasn't found, please check the installation") + +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. + + :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 + + yarp.Network.init() + return True + + def open_rpc_client_port(port_name:str)->Tuple[bool, Optional[yarp.RpcClient]]: + """ + Opens a YARP RpcClient port with the specified name. + + :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. + + :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 + + + 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/failures.py b/src/pycram/failures.py index 7513bad81..dc4f1f616 100644 --- a/src/pycram/failures.py +++ b/src/pycram/failures.py @@ -527,3 +527,14 @@ def __init__(self, link_name: str): 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_module.py b/src/pycram/process_module.py index 45d5920ff..48fc37e59 100644 --- a/src/pycram/process_module.py +++ b/src/pycram/process_module.py @@ -439,3 +439,9 @@ def close(self) -> 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 c8ca456c9..f28110635 100644 --- a/src/pycram/process_modules/__init__.py +++ b/src/pycram/process_modules/__init__.py @@ -6,6 +6,7 @@ from .stretch_process_modules import StretchManager from .robotiq_gripper_process_module import RobotiqManager from .tiago_process_modules import TiagoManager +from .icub_process_modules import ICubManager Pr2Manager() BoxyManager() @@ -13,5 +14,6 @@ HSRBManager() DefaultManager() StretchManager() +ICubManager() TiagoManager() RobotiqManager() 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..91e95b6da --- /dev/null +++ b/src/pycram/process_modules/icub_process_modules.py @@ -0,0 +1,698 @@ +import math +from .default_process_modules import DefaultMoveJoints, DefaultMoveArmJoints, DefaultMoveTCP, DefaultNavigation, \ + DefaultMoveHead, DefaultDetecting, DefaultMoveGripper + +from ..failures import RobotNotInitialized +from ..process_module import ProcessModule, ProcessModuleManager +from ..ros.logging import logerr +from ..designators.motion_designator import MoveMotion, LookingMotion, \ + DetectingMotion, MoveTCPMotion, MoveArmJointsMotion, WorldStateDetectingMotion, MoveJointsMotion, \ + MoveGripperMotion +from ..robot_description import RobotDescription +from ..datastructures.world import World +from ..datastructures.enums import Arms, ExecutionType, GripperState +from ..external_interfaces.robokudo import * + +try: + from pycram.external_interfaces.yarp_networking import * +except ImportError: + logwarn("Yarp Was Not Found. You can't use iCub. Check yarp installation") + yarp = None + pass + + + +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 = [] + for i in range(part_state.size()): + 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]] = math.degrees(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) + + 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 + + + +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. + """ + + 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 + + + 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 + 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: + self.update_hand(self.HAND_CLOSED,self.ctp_ports[0]) + else: + self.update_hand(self.HAND_OPENED, self.ctp_ports[0]) + + elif gripper == Arms.LEFT: + if required_status == GripperState.CLOSE: + self.update_hand(self.HAND_CLOSED, self.ctp_ports[1]) + else: + self.update_hand(self.HAND_OPENED, self.ctp_ports[1]) + + elif gripper ==Arms.BOTH: + if required_status == GripperState.CLOSE: + self.update_hand(self.HAND_CLOSED, self.ctp_ports[0]) + self.update_hand(self.HAND_CLOSED, self.ctp_ports[1]) + else: + self.update_hand(self.HAND_OPENED, self.ctp_ports[0]) + self.update_hand(self.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 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 + + 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 + + + else: + logwarn("port is not connected") + return False + + + + + 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" + """ + + 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 _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_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") + + + 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") + + 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[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) + + + + + +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): + obj_type = desig.object_type + return list(filter(lambda obj: obj.obj_type == obj_type, World.current_world.objects))[0] + + + + +########################################################### +######## Process Modules for the simulated iCub ########### +########################################################### + + +class iCubNavigation(DefaultNavigation): + """ + Process module for the real PR2 that sends a cartesian goal to giskard to move the robot base + """ + ... + + +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 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. + """ + + ... + + +class iCubMoveTCP(DefaultMoveTCP): + """ + Moves the tool center point of the real PR2 while avoiding all collisions + """ + + ... + + +class iCubMoveArmJoints(DefaultMoveArmJoints): + """ + Moves the arm joints of the real iCub to the given configuration while avoiding all collisions + """ + ... + + +class iCubMoveJoints(DefaultMoveJoints): + """ + Moves any joint using giskard, avoids all collisions while doint this. + """ + ... + + +class iCubMoveGripper(DefaultMoveGripper): + """ + Opens or closes the gripper of the real PR2, gripper uses an action server for this instead of giskard + """ + + ... + + +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() + + 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 + 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/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 + + 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: + loginfo("yarp network state detected") + self.config_yarp_ports() + self.connect_yarp_ports() + self.initialized = True + else: + logwarn("yarp network state not detected. ICubManager wasn't initialized correctly") + + + def navigate(self): + if ProcessModuleManager.execution_type == ExecutionType.SIMULATED: + return iCubNavigation(self._navigate_lock) + elif ProcessModuleManager.execution_type == ExecutionType.REAL: + if not self.initialized: + raise RobotNotInitialized("icub") + 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("icub") + 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("icub") + 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("icub") + 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) + + elif ProcessModuleManager.execution_type == ExecutionType.REAL: + if not self.initialized: + 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]) + + 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: + if not self.initialized: + 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]) + + 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("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]) + + def open(self): + logwarn("iCub doesn't perform open action from here") + + def close(self): + 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: + 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: + 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: + 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: + 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: + 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: + 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: + 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: + 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: + 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: + 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") + + if not connection_status: + 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: + 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: + 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: + 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: + 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: + logerr("state left_arm port couldn't connect") + return False + + return True + + + def exit(self): + if self.initialized: + self.disconnect_and_remove() + + + def disconnect_and_remove(self): + 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() + self.ctp_left_arm_client_port.interrupt() + self.state_torso_port.interrupt() + self.state_right_arm_port.interrupt() + self.state_left_arm_port.interrupt() + + 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() + 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_description.py b/src/pycram/robot_description.py index 5b8ba2aab..ab91d00ff 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 @@ -119,7 +119,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, - ignore_joints: Optional[List[str]] = None): + actuated_joints: Optional[Dict] = None,ignore_joints: Optional[List[str]] = None): + """ Initialize the RobotDescription. The URDF is loaded from the given path and used as basis for the kinematic chains. @@ -131,7 +132,9 @@ 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. :param ignore_joints: List of joint names that are not used. + """ self.name = name self.base_link = base_link @@ -142,7 +145,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]] = {} @@ -159,7 +166,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. @@ -170,6 +177,34 @@ def get_actuator_for_joint(self, joint: str) -> Optional[str]: return self.joint_actuators.get(joint) return None + + def get_actuated_joint_names(self, part_name:str) -> Optional[str]: + """ + Get the list of the actuated joint names for a given part name. + + :param part_name: part name + return: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. + + 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_name,part_index, joint_index + return None,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 new file mode 100644 index 000000000..8775e6fe3 --- /dev/null +++ b/src/pycram/robot_descriptions/icub_description.py @@ -0,0 +1,213 @@ +import math + +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('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"], + "head":["neck_pitch","neck_roll","neck_yaw", + "eyes_tilt","eyes_version","eyes_vergence"], + } + + +icub_description = RobotDescription("icub", + "root_link", + "torso_1", + "torso_pitch", + filename, + actuated_joints=actuated_joints) + +################################## 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': 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) + + + +################################## Left Gripper ################################## +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 + + + +################################## 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': 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) + + +################################## Right Gripper ################################## +right_gripper = EndEffectorDescription("right_gripper", "r_hand", "r_hand", + 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 + + + +################################## Camera ################################## +left_camera = CameraDescription("left_camera", "l_eye", 1.27, + 1.60, 0.99483, 0.75049) +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", "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], + 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) diff --git a/src/pycram/world_concepts/world_object.py b/src/pycram/world_concepts/world_object.py index 034385126..820002d1a 100644 --- a/src/pycram/world_concepts/world_object.py +++ b/src/pycram/world_concepts/world_object.py @@ -965,7 +965,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/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..80849b9d2 --- /dev/null +++ b/src/pycram/yarp_utils/yarp_joint_state_updater.py @@ -0,0 +1,385 @@ +import math + +from ..datastructures.world import World +from ..failures import YarpNetworkError +from ..robot_description import RobotDescription +from ..ros.logging import logdebug, logwarn, loginfo, logerr +try: + from ..external_interfaces.yarp_networking import * +except ImportError: + yarp = None + logwarn("no yarp installation found") + +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.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() + + 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): + logerr("Can't open the port correctly") + return False + + 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 + + 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 + + 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 + + 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 + + + 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 + + def getName(self, name): + """ + Constructs a full YARP-compatible port name for the module. + + 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. + + :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}' + + + def connect_ports(self): + """ + 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`. + + :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") + 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): + + """ + 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. + + :return 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() + + 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: + + After closing all the ports, the function returns `True` to indicate + successful closure of all resources. + + :returns `True` if all ports were successfully closed or 'false' + """ + + 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)) + + #################################################################################### + # 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. + + 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) + + 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): + """ + Initializes and runs the iCub state updater module. + + - 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. + + :param: Command-line arguments used to configure the module. ex for python calls ['/', '--robot', 'icubSim'] + + :return: 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 + + 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 8bb3cf711..11b9cc02b 100644 --- a/test/test_multiverse.py +++ b/test/test_multiverse.py @@ -49,6 +49,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()