├── rpbi_utils ├── src │ └── rpbi_utils │ │ └── __init__.py ├── setup.py ├── CMakeLists.txt ├── package.xml └── scripts │ ├── calibrate_wrench_node.py │ └── rpbi_controls_node.py ├── doc ├── .gitignore ├── images │ ├── UoE.jpg │ ├── soft.png │ ├── ATI_logo.png │ ├── KCL-Logo.png │ ├── kuka_push.png │ ├── FAROS_Logo.png │ ├── rgbd_cloud.png │ ├── Harmony_logo.png │ ├── gui-controls.png │ ├── pybullet_objects.png │ ├── human_interaction.png │ └── ik_ros_sys_overview.png ├── README.md ├── index.rst ├── Makefile ├── cite.rst ├── make.bat ├── develop.rst ├── installation.rst ├── overview.rst ├── conf.py ├── acknowledgements.rst ├── communication_with_ros.rst └── examples.rst ├── ros_pybullet_interface ├── src │ └── rpbi │ │ ├── __init__.py │ │ ├── pybullet_sensor.py │ │ ├── pybullet_urdf.py │ │ ├── pybullet_visual_object.py │ │ ├── pybullet_collision_object.py │ │ ├── pybullet_robot_links.py │ │ ├── pybullet_dynamic_object.py │ │ ├── pybullet_object_pose.py │ │ ├── pybullet_soft_body.py │ │ ├── pybullet_robot_ik.py │ │ ├── pybullet_object.py │ │ ├── pybullet_robot_urdf.py │ │ ├── pybullet_visualizer.py │ │ ├── pybullet_rgbd_sensor.py │ │ ├── ros_node.py │ │ └── pybullet_robot.py ├── msg │ ├── KeyboardEvent.msg │ ├── ResetDebugVisualizerCamera.msg │ ├── MouseEvent.msg │ ├── JointInfo.msg │ ├── CalculateInverseKinematicsProblem.msg │ └── PybulletObject.msg ├── srv │ ├── GetDebugVisualizerCamera.srv │ ├── AddPybulletObject.srv │ ├── ResetJointState.srv │ ├── ResetEffState.srv │ ├── CalculateInverseKinematics.srv │ └── RobotInfo.srv ├── setup.py ├── CMakeLists.txt └── package.xml ├── rpbi_examples ├── configs │ ├── lfd │ │ ├── keyboard_scale.yaml │ │ ├── teleop.yaml │ │ ├── keyboard_to_joy.yaml │ │ ├── visual_goal.yaml │ │ ├── pushing_box.yaml │ │ ├── kuka_lwr.yaml │ │ └── config.yaml │ ├── teleop │ │ ├── keyboard_scale.yaml │ │ ├── teleop.yaml │ │ ├── keyboard_to_joy.yaml │ │ ├── kuka_lwr.yaml │ │ └── config.yaml │ ├── nsl_haption │ │ ├── keyboard_scale.yaml │ │ ├── keyboard_to_joy.yaml │ │ ├── box.yaml │ │ ├── kuka_lwr.yaml │ │ └── config.yaml │ ├── human_interaction │ │ ├── keyboard_scale.yaml │ │ ├── spacenav_scale.yaml │ │ ├── keyboard_to_joy.yaml │ │ ├── teleop.yaml │ │ ├── ik.yaml │ │ ├── box.yaml │ │ ├── kuka_lwr.yaml │ │ └── config.yaml │ ├── soft_body │ │ ├── cube.png │ │ ├── plane.yaml │ │ ├── cube.yaml │ │ ├── torus_deform.yaml │ │ ├── torus_textured.mtl │ │ ├── floor.yaml │ │ ├── cube.mtl │ │ ├── plane.mtl │ │ ├── plane.obj │ │ ├── torus.yaml │ │ ├── torus_deform.urdf │ │ ├── plane.urdf │ │ ├── cube.urdf │ │ ├── cube.obj │ │ └── config.yaml │ ├── nsl │ │ ├── bc_d20_1664663182235112824.regr │ │ ├── bc_d50_1664663453379648783.regr │ │ ├── goal_box.yaml │ │ ├── box.yaml │ │ ├── kuka_lwr.yaml │ │ └── config.yaml │ ├── run_pose │ │ ├── table.yaml │ │ ├── whiteboard.yaml │ │ ├── cup.yaml │ │ ├── human_model.yaml │ │ ├── cup.mtl │ │ ├── kinova.yaml │ │ ├── kuka_lwr.yaml │ │ ├── config_talos.yaml │ │ ├── config_human_model.yaml │ │ ├── config_kuka_lwr.yaml │ │ ├── cup.urdf │ │ ├── config_kinova.yaml │ │ └── talos.yaml │ ├── interpolation │ │ ├── visual_wpt1.yaml │ │ ├── visual_wpt2.yaml │ │ ├── visual_wpt3.yaml │ │ ├── kuka_lwr.yaml │ │ ├── traj_lwr.yaml │ │ └── config.yaml │ ├── basic_example_nextage │ │ ├── table.yaml │ │ ├── nextage.yaml │ │ └── config.yaml │ ├── pybullet_objects_example │ │ ├── visual_sphere.yaml │ │ ├── dynamic_box.yaml │ │ └── config.yaml │ ├── basic_example_human_model │ │ ├── human_model.yaml │ │ ├── kuka_lwr.yaml │ │ └── config.yaml │ ├── basic_example_talos │ │ ├── talos.yaml │ │ └── config.yaml │ ├── floor.yaml │ ├── basic_example_kinova │ │ ├── kinova.yaml │ │ └── config.yaml │ ├── optas_kuka_lwr_expr │ │ ├── kuka_lwr.yaml │ │ └── config.yaml │ ├── optas_kuka_lwr_expr2 │ │ ├── kuka_lwr.yaml │ │ └── config.yaml │ ├── run_kuka │ │ ├── kuka_lwr.yaml │ │ └── config.yaml │ ├── basic_example_kuka_lwr │ │ ├── kuka_lwr.yaml │ │ └── config.yaml │ └── rgbd.rviz ├── launch │ ├── soft_body.launch │ ├── optas_kuka_lwr_expr.launch │ ├── optas_kuka_lwr_expr2.launch │ ├── run_pose_human_model.launch │ ├── run_pose_talos.launch │ ├── run_pose_kuka_lwr.launch │ ├── basic_example_talos.launch │ ├── basic_example_human_model.launch │ ├── run_kuka.launch │ ├── pybullet_objects_example.launch │ ├── pybullet_rgbd_example.launch │ ├── basic_example_kuka_lwr.launch │ ├── basic_example_nextage.launch │ ├── nsl_haption.launch │ ├── run_pose_kinova.launch │ ├── basic_example_kinova.launch │ ├── nsl.launch │ ├── interpolation.launch │ ├── teleop.launch │ ├── human_interaction.launch │ └── lfd.launch ├── CMakeLists.txt ├── package.xml └── scripts │ ├── pybullet_objects_example_node.py │ ├── orbit_node.py │ ├── run_human_interaction.py │ ├── basic_robot_example_node.py │ ├── run_teleop.py │ └── run_interpolation.py ├── .gitignore ├── LICENSE ├── README.md └── install.sh /rpbi_utils/src/rpbi_utils/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /doc/.gitignore: -------------------------------------------------------------------------------- 1 | # Build directory 2 | _build/ -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/__init__.py: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /rpbi_examples/configs/lfd/keyboard_scale.yaml: -------------------------------------------------------------------------------- 1 | axes_idx: [1, 0, 2] 2 | scale: 0.4 3 | -------------------------------------------------------------------------------- /rpbi_examples/configs/teleop/keyboard_scale.yaml: -------------------------------------------------------------------------------- 1 | axes_idx: [1, 0, 2] 2 | scale: 0.4 3 | -------------------------------------------------------------------------------- /rpbi_examples/configs/nsl_haption/keyboard_scale.yaml: -------------------------------------------------------------------------------- 1 | axes_idx: [1, 0, 2] 2 | scale: 1. 3 | -------------------------------------------------------------------------------- /rpbi_examples/configs/human_interaction/keyboard_scale.yaml: -------------------------------------------------------------------------------- 1 | axes_idx: [1, 0, 2] 2 | scale: 0.4 3 | -------------------------------------------------------------------------------- /doc/images/UoE.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/doc/images/UoE.jpg -------------------------------------------------------------------------------- /doc/images/soft.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/doc/images/soft.png -------------------------------------------------------------------------------- /doc/images/ATI_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/doc/images/ATI_logo.png -------------------------------------------------------------------------------- /doc/images/KCL-Logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/doc/images/KCL-Logo.png -------------------------------------------------------------------------------- /doc/images/kuka_push.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/doc/images/kuka_push.png -------------------------------------------------------------------------------- /ros_pybullet_interface/msg/KeyboardEvent.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint64 key 3 | uint8 state 4 | string state_str -------------------------------------------------------------------------------- /rpbi_examples/configs/human_interaction/spacenav_scale.yaml: -------------------------------------------------------------------------------- 1 | axes_idx: [0, 1, 2] 2 | scale: [0.0, 0.0, -0.2] 3 | -------------------------------------------------------------------------------- /rpbi_examples/configs/lfd/teleop.yaml: -------------------------------------------------------------------------------- 1 | parent_frame_id: "teleop_origin" 2 | target_frame_id: "teleop_target" 3 | -------------------------------------------------------------------------------- /doc/images/FAROS_Logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/doc/images/FAROS_Logo.png -------------------------------------------------------------------------------- /doc/images/rgbd_cloud.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/doc/images/rgbd_cloud.png -------------------------------------------------------------------------------- /rpbi_examples/configs/teleop/teleop.yaml: -------------------------------------------------------------------------------- 1 | parent_frame_id: "teleop_origin" 2 | target_frame_id: "teleop_target" 3 | -------------------------------------------------------------------------------- /doc/images/Harmony_logo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/doc/images/Harmony_logo.png -------------------------------------------------------------------------------- /doc/images/gui-controls.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/doc/images/gui-controls.png -------------------------------------------------------------------------------- /doc/images/pybullet_objects.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/doc/images/pybullet_objects.png -------------------------------------------------------------------------------- /rpbi_examples/configs/lfd/keyboard_to_joy.yaml: -------------------------------------------------------------------------------- 1 | axes: 2 | - ["RIGHT", "LEFT"] 3 | - ["UP", "DOWN"] 4 | - ["b", "f"] 5 | -------------------------------------------------------------------------------- /rpbi_examples/configs/teleop/keyboard_to_joy.yaml: -------------------------------------------------------------------------------- 1 | axes: 2 | - ["RIGHT", "LEFT"] 3 | - ["UP", "DOWN"] 4 | - ["b", "f"] 5 | -------------------------------------------------------------------------------- /doc/images/human_interaction.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/doc/images/human_interaction.png -------------------------------------------------------------------------------- /doc/images/ik_ros_sys_overview.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/doc/images/ik_ros_sys_overview.png -------------------------------------------------------------------------------- /rpbi_examples/configs/nsl_haption/keyboard_to_joy.yaml: -------------------------------------------------------------------------------- 1 | axes: 2 | - ["RIGHT", "LEFT"] 3 | - ["UP", "DOWN"] 4 | - ["k", "m"] 5 | -------------------------------------------------------------------------------- /ros_pybullet_interface/srv/GetDebugVisualizerCamera.srv: -------------------------------------------------------------------------------- 1 | --- 2 | ros_pybullet_interface/ResetDebugVisualizerCamera debug_visualizer_config -------------------------------------------------------------------------------- /rpbi_examples/configs/human_interaction/keyboard_to_joy.yaml: -------------------------------------------------------------------------------- 1 | axes: 2 | - ["RIGHT", "LEFT"] 3 | - ["UP", "DOWN"] 4 | - ["b", "f"] 5 | -------------------------------------------------------------------------------- /ros_pybullet_interface/srv/AddPybulletObject.srv: -------------------------------------------------------------------------------- 1 | ros_pybullet_interface/PybulletObject pybullet_object 2 | --- 3 | bool success 4 | string message -------------------------------------------------------------------------------- /ros_pybullet_interface/srv/ResetJointState.srv: -------------------------------------------------------------------------------- 1 | sensor_msgs/JointState joint_state 2 | float64 duration 3 | --- 4 | string message 5 | bool success -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/cube.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/rpbi_examples/configs/soft_body/cube.png -------------------------------------------------------------------------------- /rpbi_examples/configs/human_interaction/teleop.yaml: -------------------------------------------------------------------------------- 1 | parent_frame_id: "teleop_origin" 2 | target_frame_id: "teleop_target" 3 | zlim: [-10.0, 0.08] 4 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_sensor.py: -------------------------------------------------------------------------------- 1 | from .pybullet_object import PybulletObject 2 | 3 | class PybulletSensor(PybulletObject): 4 | pass 5 | -------------------------------------------------------------------------------- /ros_pybullet_interface/msg/ResetDebugVisualizerCamera.msg: -------------------------------------------------------------------------------- 1 | float64 cameraDistance 2 | float64 cameraYaw 3 | float64 cameraPitch 4 | float64[] cameraTargetPosition -------------------------------------------------------------------------------- /ros_pybullet_interface/srv/ResetEffState.srv: -------------------------------------------------------------------------------- 1 | ros_pybullet_interface/CalculateInverseKinematicsProblem problem 2 | float64 duration 3 | --- 4 | string message 5 | bool success -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/plane.yaml: -------------------------------------------------------------------------------- 1 | name: "plane" 2 | 3 | loadURDF: 4 | fileName: "{rpbi_examples}/configs/soft_body/plane.urdf" 5 | basePosition: [0, 0, 1] 6 | -------------------------------------------------------------------------------- /rpbi_examples/configs/nsl/bc_d20_1664663182235112824.regr: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/rpbi_examples/configs/nsl/bc_d20_1664663182235112824.regr -------------------------------------------------------------------------------- /rpbi_examples/configs/nsl/bc_d50_1664663453379648783.regr: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/cmower/ros_pybullet_interface/HEAD/rpbi_examples/configs/nsl/bc_d50_1664663453379648783.regr -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/table.yaml: -------------------------------------------------------------------------------- 1 | name: "table" 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_BOX" 5 | halfExtents: [0.1, 0.1, 0.1] 6 | 7 | object_tf: 8 | tf_id: "rpbi/table" 9 | -------------------------------------------------------------------------------- /ros_pybullet_interface/srv/CalculateInverseKinematics.srv: -------------------------------------------------------------------------------- 1 | ros_pybullet_interface/CalculateInverseKinematicsProblem problem 2 | --- 3 | bool success 4 | string message 5 | sensor_msgs/JointState solution -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/cube.yaml: -------------------------------------------------------------------------------- 1 | name: "cube" 2 | 3 | loadURDF: 4 | fileName: "{rpbi_examples}/configs/soft_body/cube.urdf" 5 | basePosition: [0, 1, 2] 6 | useMaximalCoordinates: true 7 | -------------------------------------------------------------------------------- /rpbi_examples/configs/human_interaction/ik.yaml: -------------------------------------------------------------------------------- 1 | robot_name: "kuka_lwr" 2 | link_name: "end_effector_sponge" 3 | 4 | parent_frame_id: "rpbi/kuka_lwr/lwr_arm_0_link" 5 | child_frame_id: "teleop_target" 6 | -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/whiteboard.yaml: -------------------------------------------------------------------------------- 1 | name: "whiteboard" 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_BOX" 5 | halfExtents: [0.3, 0.2, 0.025] 6 | 7 | object_tf: 8 | tf_id: "rpbi/whiteboard" 9 | -------------------------------------------------------------------------------- /ros_pybullet_interface/msg/MouseEvent.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | uint8 event_type 3 | string event_type_str 4 | float64 mouse_pos_x 5 | float64 mouse_pos_y 6 | uint8 button_index 7 | uint8 button_state 8 | uint8 button_state_str -------------------------------------------------------------------------------- /rpbi_examples/configs/interpolation/visual_wpt1.yaml: -------------------------------------------------------------------------------- 1 | name: 'visual_wpt1' 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_SPHERE" 5 | radius: 0.025 6 | rgbaColor: [0.0, 1.0, 0, 0.6] 7 | 8 | object_tf: 9 | tf_id: "rpbi/visual_wpt1" 10 | -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/torus_deform.yaml: -------------------------------------------------------------------------------- 1 | name: "torus_deform" 2 | 3 | loadURDF: 4 | fileName: "{rpbi_examples}/configs/soft_body/torus_deform.urdf" 5 | basePosition: [0, 0, 2] #[0, -0.6, 2] 6 | flags: "URDF_USE_SELF_COLLISION" 7 | -------------------------------------------------------------------------------- /rpbi_examples/configs/interpolation/visual_wpt2.yaml: -------------------------------------------------------------------------------- 1 | name: 'visual_wpt2' 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_SPHERE" 5 | radius: 0.025 6 | rgbaColor: [0.0, 0.0, 1.0, 0.6] 7 | 8 | object_tf: 9 | tf_id: "rpbi/visual_wpt2" 10 | -------------------------------------------------------------------------------- /rpbi_examples/configs/interpolation/visual_wpt3.yaml: -------------------------------------------------------------------------------- 1 | name: 'visual_wpt3' 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_SPHERE" 5 | radius: 0.025 6 | rgbaColor: [1.0, 0.0, 0.0, 0.6] 7 | 8 | object_tf: 9 | tf_id: "rpbi/visual_wpt3" 10 | -------------------------------------------------------------------------------- /rpbi_examples/configs/basic_example_nextage/table.yaml: -------------------------------------------------------------------------------- 1 | name: "table" 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_BOX" 5 | halfExtents: [0.5, 0.9, 0.02] 6 | rgbaColor: [0.1, 0.23, 0.8, 1.0] 7 | 8 | object_tf: 9 | tf_id: "rpbi/table" 10 | -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/torus_textured.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'None' 2 | # Material Count: 1 3 | 4 | newmtl None 5 | Ns 0 6 | Ka 0.000000 0.000000 0.000000 7 | Kd 0.8 0.8 0.8 8 | Ks 0.8 0.8 0.8 9 | d 1 10 | illum 2 11 | map_Kd ../cube.png 12 | -------------------------------------------------------------------------------- /rpbi_examples/configs/lfd/visual_goal.yaml: -------------------------------------------------------------------------------- 1 | name: 'visual_sphere' 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_BOX" 5 | halfExtents: [0.1, 0.1, 0.075] 6 | rgbaColor: [0.0, 1.0, 0, 0.6] 7 | 8 | 9 | object_tf: 10 | tf_id: "rpbi/visual_goal" 11 | -------------------------------------------------------------------------------- /rpbi_examples/configs/pybullet_objects_example/visual_sphere.yaml: -------------------------------------------------------------------------------- 1 | name: 'visual_sphere' 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_SPHERE" 5 | radius: 0.2 6 | rgbaColor: [0.0, 1.0, 1, 1.0] 7 | 8 | object_tf: 9 | tf_id: "figure_eight" 10 | hz: 30 11 | -------------------------------------------------------------------------------- /ros_pybullet_interface/srv/RobotInfo.srv: -------------------------------------------------------------------------------- 1 | --- 2 | string robot_name 3 | string root_link_name 4 | int64 bodyUniqueId 5 | int64 numJoints 6 | int64 numDof 7 | ros_pybullet_interface/JointInfo[] joint_info 8 | string[] enabled_ft_sensors 9 | sensor_msgs/JointState current_joint_state -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/cup.yaml: -------------------------------------------------------------------------------- 1 | name: "cup" 2 | 3 | loadURDF: 4 | fileName: "{rpbi_examples}/configs/run_pose/cup.urdf" 5 | basePosition: [0.4, 0.4, 0.4] 6 | # baseOrientation: [0. , 0.70710678, 0. , 0.70710678] 7 | useMaximalCoordinates: true 8 | useFixedBase: 1 9 | -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/floor.yaml: -------------------------------------------------------------------------------- 1 | name: "floor" 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_PLANE" 5 | planeNormal: [0, 0, 1] 6 | 7 | createCollisionShape: 8 | shapeType: "GEOM_PLANE" 9 | planeNormal: [0, 0, 1] 10 | 11 | changeDynamics: 12 | lateralFriction: 1.0 13 | -------------------------------------------------------------------------------- /rpbi_examples/launch/soft_body.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /rpbi_examples/launch/optas_kuka_lwr_expr.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /rpbi_examples/launch/optas_kuka_lwr_expr2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/cube.mtl: -------------------------------------------------------------------------------- 1 | newmtl cube 2 | Ns 10.0000 3 | Ni 1.5000 4 | d 1.0000 5 | Tr 0.0000 6 | Tf 1.0000 1.0000 1.0000 7 | illum 2 8 | Ka 0.0000 0.0000 0.0000 9 | Kd 0.5880 0.5880 0.5880 10 | Ks 0.0000 0.0000 0.0000 11 | Ke 0.0000 0.0000 0.0000 12 | map_Ka cube.tga 13 | map_Kd cube.png 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/plane.mtl: -------------------------------------------------------------------------------- 1 | newmtl Material 2 | Ns 10.0000 3 | Ni 1.5000 4 | d 1.0000 5 | Tr 0.0000 6 | Tf 1.0000 1.0000 1.0000 7 | illum 2 8 | Ka 0.0000 0.0000 0.0000 9 | Kd 0.5880 0.5880 0.5880 10 | Ks 0.0000 0.0000 0.0000 11 | Ke 0.0000 0.0000 0.0000 12 | map_Ka cube.tga 13 | map_Kd checker_blue.png 14 | 15 | 16 | -------------------------------------------------------------------------------- /rpbi_examples/launch/run_pose_human_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /doc/README.md: -------------------------------------------------------------------------------- 1 | # Documentation 2 | 3 | To build the documentation follow the following. 4 | 5 | 1. Install dependencies: 6 | * `$ pip install Sphinx` 7 | * `$ pip install sphinx-rtd-theme` 8 | 2. `$ cd path/to/ros_pybullet_interface/doc` 9 | 3. Make 10 | * HTML: `make html` 11 | * PDF LaTeX: `make latexpdf` 12 | 13 | You will find the compiled files in the `_build` directory. 14 | -------------------------------------------------------------------------------- /rpbi_examples/configs/nsl/goal_box.yaml: -------------------------------------------------------------------------------- 1 | name: "goal_box" 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_BOX" 5 | halfExtents: [0.125, 0.125, 0.125] 6 | rgbaColor: [0, 1.0, 0., 0.5] 7 | 8 | createCollisionShape: 9 | shapeType: "GEOM_BOX" 10 | # halfExtents: [0.1, 0.1, 0.075] 11 | halfExtents: [0.125, 0.125, 0.125] 12 | 13 | 14 | object_tf: 15 | tf_id: "goal_box" 16 | hz: 10 17 | -------------------------------------------------------------------------------- /rpbi_examples/configs/basic_example_nextage/nextage.yaml: -------------------------------------------------------------------------------- 1 | name: "nextage" 2 | 3 | loadURDF: 4 | fileName: "robot_description" 5 | useFixedBase: 1 6 | basePosition: [0., 0., 0.85] 7 | 8 | is_visual_robot: false 9 | 10 | setJointMotorControlArray: 11 | controlMode: "POSITION_CONTROL" 12 | 13 | publish_joint_state_frequency: 30 14 | 15 | broadcast_link_states: true 16 | brodcast_link_states_frequency: 30 17 | -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/human_model.yaml: -------------------------------------------------------------------------------- 1 | name: "human" 2 | 3 | loadURDF: 4 | fileName: "{rpbi_examples}/robots/human_model.urdf" 5 | useFixedBase: 1 6 | basePosition: [0.5, 0., 1.] 7 | 8 | is_visual_robot: true 9 | 10 | setJointMotorControlArray: 11 | controlMode: "POSITION_CONTROL" 12 | 13 | publish_joint_state_frequency: 30 14 | 15 | broadcast_link_states: true 16 | brodcast_link_states_frequency: 30 17 | -------------------------------------------------------------------------------- /rpbi_examples/configs/basic_example_human_model/human_model.yaml: -------------------------------------------------------------------------------- 1 | name: "human" 2 | 3 | loadURDF: 4 | fileName: "{rpbi_examples}/robots/human_model.urdf" 5 | useFixedBase: 1 6 | basePosition: [0.5, 0., 1.] 7 | 8 | is_visual_robot: false 9 | 10 | setJointMotorControlArray: 11 | controlMode: "POSITION_CONTROL" 12 | 13 | publish_joint_state_frequency: 30 14 | 15 | broadcast_link_states: true 16 | brodcast_link_states_frequency: 30 17 | -------------------------------------------------------------------------------- /rpbi_examples/configs/basic_example_talos/talos.yaml: -------------------------------------------------------------------------------- 1 | name: "talos" 2 | 3 | loadURDF: 4 | fileName: 'robot_description' 5 | useFixedBase: 1 # use 1 (fixed base) only for demo 6 | basePosition: [0, 0, 1.1] 7 | 8 | is_visual_robot: false 9 | 10 | setJointMotorControlArray: 11 | controlMode: "POSITION_CONTROL" 12 | 13 | publish_joint_state_frequency: 30 14 | 15 | broadcast_link_states: true 16 | brodcast_link_states_frequency: 30 17 | -------------------------------------------------------------------------------- /rpbi_examples/launch/run_pose_talos.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/plane.obj: -------------------------------------------------------------------------------- 1 | # Blender v2.66 (sub 1) OBJ File: '' 2 | # www.blender.org 3 | mtllib plane.mtl 4 | o Plane 5 | v 15.000000 -15.000000 0.000000 6 | v 15.000000 15.000000 0.000000 7 | v -15.000000 15.000000 0.000000 8 | v -15.000000 -15.000000 0.000000 9 | 10 | vt 15.000000 0.000000 11 | vt 15.000000 15.000000 12 | vt 0.000000 15.000000 13 | vt 0.000000 0.000000 14 | 15 | usemtl Material 16 | s off 17 | f 1/1 2/2 3/3 18 | f 1/1 3/3 4/4 19 | -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/torus.yaml: -------------------------------------------------------------------------------- 1 | name: "torus" 2 | 3 | loadSoftBody: 4 | fileName: "{rpbi_examples}/configs/soft_body/torus_textured.obj" 5 | basePosition: [0, 0, 2] 6 | simFileName: "{rpbi_examples}/configs/soft_body/torus.vtk" 7 | mass: 3 8 | useNeoHookean: 1 9 | NeoHookeanMu: 180 10 | NeoHookeanLambda: 600 11 | NeoHookeanDamping: 0.01 12 | collisionMargin: 0.006 13 | useSelfCollision: 1 14 | frictionCoeff: 0.5 15 | repulsionStiffness: 800 16 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_urdf.py: -------------------------------------------------------------------------------- 1 | from .pybullet_object import PybulletObject 2 | from .pybullet_robot_urdf import URDF 3 | 4 | class PybulletURDF(PybulletObject): 5 | 6 | def init(self): 7 | self.is_visual_robot = False # HACK: so we can use the URDF class in pybullet_robot_urdf 8 | self.urdf = URDF(self) 9 | self.body_unique_id = self.urdf.load() 10 | 11 | @property 12 | def loadURDF(self): 13 | return self.config['loadURDF'] 14 | 15 | 16 | -------------------------------------------------------------------------------- /rpbi_utils/setup.py: -------------------------------------------------------------------------------- 1 | ## ------------------------------------------------------------ ## 2 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD ! ## 3 | ## ------------------------------------------------------------ ## 4 | from distutils.core import setup 5 | from catkin_pkg.python_setup import generate_distutils_setup 6 | 7 | # fetch values from package.xml 8 | setup_args = generate_distutils_setup( 9 | packages=['rpbi_utils'], 10 | package_dir={'': 'src'}) 11 | 12 | setup(**setup_args) 13 | -------------------------------------------------------------------------------- /ros_pybullet_interface/setup.py: -------------------------------------------------------------------------------- 1 | ## ------------------------------------------------------------ ## 2 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD ! ## 3 | ## ------------------------------------------------------------ ## 4 | from distutils.core import setup 5 | from catkin_pkg.python_setup import generate_distutils_setup 6 | 7 | # fetch values from package.xml 8 | setup_args = generate_distutils_setup( 9 | packages=['rpbi'], 10 | package_dir={'': 'src'}) 11 | 12 | setup(**setup_args) 13 | -------------------------------------------------------------------------------- /rpbi_examples/configs/floor.yaml: -------------------------------------------------------------------------------- 1 | name: "floor" 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_PLANE" 5 | planeNormal: [0, 0, 1] 6 | 7 | createCollisionShape: 8 | shapeType: "GEOM_PLANE" 9 | planeNormal: [0, 0, 1] 10 | 11 | changeDynamics: 12 | lateralFriction: 1.0 13 | spinningFriction: 0.0 14 | rollingFriction: 0.0 15 | restitution: 0.0 16 | linearDamping: 0.04 17 | angularDamping: 0.04 18 | contactStiffness: 2000.0 19 | contactDamping: 0.7 20 | 21 | object_tf: 22 | tf_id: "rpbi/floor" -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/torus_deform.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /doc/index.rst: -------------------------------------------------------------------------------- 1 | 2 | ROS-PyBullet Interface 3 | ====================== 4 | 5 | The ROS-PyBullet Interface is a bridge between the popular physics library `PyBullet `_ and the `Robot Operating System (ROS) `_. 6 | 7 | .. toctree:: 8 | :maxdepth: 1 9 | 10 | overview 11 | installation 12 | examples 13 | main_configuration 14 | create_virtual_worlds 15 | communication_with_ros 16 | additional_features 17 | cite 18 | develop 19 | acknowledgements 20 | -------------------------------------------------------------------------------- /ros_pybullet_interface/msg/JointInfo.msg: -------------------------------------------------------------------------------- 1 | int64 jointIndex 2 | string jointName 3 | int64 jointType 4 | string jointTypeStr 5 | int64 qIndex 6 | int64 uIndex 7 | int64 flags 8 | float64 jointDamping 9 | float64 jointFriction 10 | float64 jointLowerLimit 11 | float64 jointUpperLimit 12 | float64 jointMaxForce 13 | float64 jointMaxVelocity 14 | string linkName 15 | geometry_msgs/Point jointAxis 16 | geometry_msgs/Point parentFramePos 17 | geometry_msgs/Quaternion parentFrameOrn 18 | int64 parentIndex 19 | 20 | bool is_fixed 21 | bool is_revolute -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/cup.mtl: -------------------------------------------------------------------------------- 1 | # Blender MTL File: 'untitled.blend' 2 | # Material Count: 2 3 | 4 | newmtl Material.001 5 | Ns 728.999961 6 | Ka 1.000000 1.000000 1.000000 7 | Kd 0.151028 0.341754 0.800000 8 | Ks 0.500000 0.500000 0.500000 9 | Ke 0.000000 0.000000 0.000000 10 | Ni 1.450000 11 | d 1.000000 12 | illum 2 13 | 14 | newmtl Material.002 15 | Ns 728.999998 16 | Ka 1.000000 1.000000 1.000000 17 | Kd 0.733810 0.646705 0.672786 18 | Ks 0.500000 0.500000 0.500000 19 | Ke 0.000000 0.000000 0.000000 20 | Ni 1.450000 21 | d 1.000000 22 | illum 2 23 | -------------------------------------------------------------------------------- /rpbi_utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rpbi_utils) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | tf2 7 | tf2_ros 8 | geometry_msgs 9 | std_msgs 10 | ) 11 | 12 | catkin_python_setup() 13 | 14 | catkin_package() 15 | 16 | include_directories( 17 | ${catkin_INCLUDE_DIRS} 18 | ) 19 | 20 | catkin_install_python(PROGRAMS 21 | scripts/rpbi_controls_node.py 22 | scripts/interpolation_node.py 23 | scripts/calibrate_wrench_node.py 24 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 25 | ) 26 | -------------------------------------------------------------------------------- /ros_pybullet_interface/msg/CalculateInverseKinematicsProblem.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header 2 | string link_name 3 | float64[] targetPosition 4 | float64[] targetOrientation 5 | float64[] lowerLimits 6 | float64[] upperLimits 7 | float64[] jointRanges 8 | float64[] resetPoses 9 | float64[] jointDamping 10 | int64 solver # use 0 for p.IK_DLS or 1 for p.IK_SDLS 11 | float64[] currentPosition 12 | int64 maxNumIterations 13 | float64 residualThreshold 14 | 15 | float64 dt # used to calculate joint velocity (vel=(solution-currentPosition)/dt), this must be given if robot is in velocity control -------------------------------------------------------------------------------- /rpbi_examples/configs/lfd/pushing_box.yaml: -------------------------------------------------------------------------------- 1 | name: "pushing_box" 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_BOX" 5 | halfExtents: [0.1, 0.1, 0.075] 6 | rgbaColor: [0.9, 1.0, 0.3, 1.0] 7 | 8 | createCollisionShape: 9 | shapeType: "GEOM_BOX" 10 | halfExtents: [0.1, 0.1, 0.075] 11 | 12 | baseMass: 1.0 13 | basePosition: [-0.6, 0.0, 0.3] 14 | 15 | changeDynamics: 16 | lateralFriction: 1.0 17 | spinningFriction: 0.0 18 | rollingFriction: 0.0 19 | restitution: 0.0 20 | linearDamping: 0.04 21 | angularDamping: 0.04 22 | contactStiffness: 2000.0 23 | contactDamping: 0.7 24 | -------------------------------------------------------------------------------- /rpbi_examples/configs/human_interaction/box.yaml: -------------------------------------------------------------------------------- 1 | name: "box" 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_BOX" 5 | halfExtents: [0.1, 0.1, 0.06] 6 | rgbaColor: [0.1, 0.2, 0.4, 1.0] 7 | 8 | 9 | createCollisionShape: 10 | shapeType: "GEOM_BOX" 11 | halfExtents: [0.1, 0.1, 0.06] 12 | 13 | 14 | changeDynamics: 15 | lateralFriction: 1.0 16 | spinningFriction: 0.0 17 | rollingFriction: 0.0 18 | restitution: 0.0 19 | linearDamping: 0.04 20 | angularDamping: 0.04 21 | contactStiffness: 2000.0 22 | contactDamping: 0.7 23 | 24 | object_tf: 25 | tf_id: "box" 26 | broadcast_tf: true 27 | -------------------------------------------------------------------------------- /rpbi_examples/launch/run_pose_kuka_lwr.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /rpbi_examples/configs/nsl_haption/box.yaml: -------------------------------------------------------------------------------- 1 | name: "pushing_box" 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_BOX" 5 | halfExtents: [0.125, 0.125, 0.125] 6 | rgbaColor: [0.9, 1.0, 0.3, 1.0] 7 | 8 | createCollisionShape: 9 | shapeType: "GEOM_BOX" 10 | # halfExtents: [0.1, 0.1, 0.075] 11 | halfExtents: [0.125, 0.125, 0.125] 12 | 13 | baseMass: 2. 14 | basePosition: [-0.6, 0.225, 0.3] 15 | 16 | changeDynamics: 17 | lateralFriction: 1.0 18 | spinningFriction: 0.0 19 | rollingFriction: 0.0 20 | restitution: 0.0 21 | linearDamping: 0.04 22 | angularDamping: 0.04 23 | contactStiffness: 2000.0 24 | contactDamping: 0.7 25 | -------------------------------------------------------------------------------- /rpbi_examples/launch/basic_example_talos.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /rpbi_examples/configs/basic_example_kinova/kinova.yaml: -------------------------------------------------------------------------------- 1 | name: "kinova" 2 | 3 | # 4 | # This example requires the kortex_descrition ROS package. 5 | # https://github.com/Kinovarobotics/ros_kortex/tree/noetic-devel/kortex_description 6 | # 7 | 8 | loadURDF: 9 | fileName: "robot_description" # gets urdf from ros parameter 10 | useFixedBase: 1 11 | 12 | # initial_joint_position: 13 | 14 | # initial_revolute_joint_positions_are_deg: true 15 | is_visual_robot: false 16 | 17 | setJointMotorControlArray: 18 | controlMode: "POSITION_CONTROL" 19 | 20 | publish_joint_state_frequency: 30 21 | 22 | broadcast_link_states: true 23 | brodcast_link_states_frequency: 30 24 | -------------------------------------------------------------------------------- /rpbi_examples/configs/lfd/kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | name: "kuka_lwr" 2 | 3 | loadURDF: 4 | fileName: "{ik_ros_examples}/robots/kuka_lwr.urdf" 5 | useFixedBase: 1 6 | 7 | initial_joint_position: 8 | lwr_arm_0_joint: 0.0 9 | lwr_arm_1_joint: 30.0 10 | lwr_arm_2_joint: 0.0 11 | lwr_arm_3_joint: -90.0 12 | lwr_arm_4_joint: 0.0 13 | lwr_arm_5_joint: 60.0 14 | lwr_arm_6_joint: 0.0 15 | 16 | initial_revolute_joint_positions_are_deg: true 17 | is_visual_robot: false 18 | 19 | setJointMotorControlArray: 20 | controlMode: "POSITION_CONTROL" 21 | 22 | publish_joint_state_frequency: 30 23 | 24 | broadcast_link_states: true 25 | broadcast_link_states_frequency: 30 26 | -------------------------------------------------------------------------------- /rpbi_examples/configs/teleop/kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | name: "kuka_lwr" 2 | 3 | loadURDF: 4 | fileName: "{ik_ros_examples}/robots/kuka_lwr.urdf" 5 | useFixedBase: 1 6 | 7 | initial_joint_position: 8 | lwr_arm_0_joint: 0.0 9 | lwr_arm_1_joint: 30.0 10 | lwr_arm_2_joint: 0.0 11 | lwr_arm_3_joint: -90.0 12 | lwr_arm_4_joint: 0.0 13 | lwr_arm_5_joint: 60.0 14 | lwr_arm_6_joint: 0.0 15 | 16 | initial_revolute_joint_positions_are_deg: true 17 | is_visual_robot: false 18 | 19 | setJointMotorControlArray: 20 | controlMode: "POSITION_CONTROL" 21 | 22 | publish_joint_state_frequency: 30 23 | 24 | broadcast_link_states: true 25 | broadcast_link_states_frequency: 30 26 | -------------------------------------------------------------------------------- /rpbi_examples/configs/interpolation/kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | name: "kuka_lwr" 2 | 3 | loadURDF: 4 | fileName: "{ik_ros_examples}/robots/kuka_lwr.urdf" 5 | useFixedBase: 1 6 | 7 | initial_joint_position: 8 | lwr_arm_0_joint: 0.0 9 | lwr_arm_1_joint: 30.0 10 | lwr_arm_2_joint: 0.0 11 | lwr_arm_3_joint: -90.0 12 | lwr_arm_4_joint: 0.0 13 | lwr_arm_5_joint: 60.0 14 | lwr_arm_6_joint: 0.0 15 | 16 | initial_revolute_joint_positions_are_deg: true 17 | is_visual_robot: false 18 | 19 | setJointMotorControlArray: 20 | controlMode: "POSITION_CONTROL" 21 | 22 | publish_joint_state_frequency: 30 23 | 24 | broadcast_link_states: true 25 | broadcast_link_states_frequency: 30 26 | -------------------------------------------------------------------------------- /rpbi_examples/configs/optas_kuka_lwr_expr/kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | name: "kuka_lwr" 2 | 3 | loadURDF: 4 | fileName: "{ik_ros_examples}/robots/kuka_lwr.urdf" 5 | useFixedBase: 1 6 | 7 | initial_joint_position: 8 | lwr_arm_0_joint: 0.0 9 | lwr_arm_1_joint: 30.0 10 | lwr_arm_2_joint: 0.0 11 | lwr_arm_3_joint: -90.0 12 | lwr_arm_4_joint: 0.0 13 | lwr_arm_5_joint: 60.0 14 | lwr_arm_6_joint: 0.0 15 | 16 | initial_revolute_joint_positions_are_deg: true 17 | is_visual_robot: false 18 | 19 | setJointMotorControlArray: 20 | controlMode: "POSITION_CONTROL" 21 | 22 | publish_joint_state_frequency: 30 23 | 24 | broadcast_link_states: true 25 | broadcast_link_states_frequency: 30 26 | -------------------------------------------------------------------------------- /rpbi_examples/configs/pybullet_objects_example/dynamic_box.yaml: -------------------------------------------------------------------------------- 1 | name: "dynamic_box" 2 | 3 | createVisualShape: 4 | shapeType: "GEOM_BOX" 5 | halfExtents: [0.3, 0.15, 0.1] 6 | rgbaColor: [0.9, 1.0, 0.3, 1.0] 7 | 8 | createCollisionShape: 9 | shapeType: "GEOM_BOX" 10 | halfExtents: [0.3, 0.15, 0.1] 11 | 12 | baseMass: 0.5 13 | basePosition: [0.0, 0.0, 1.0] 14 | 15 | resetBaseVelocity: 16 | linearVelocity: [0.0, 0, 1.5] 17 | 18 | changeDynamics: 19 | lateralFriction: 1.0 20 | spinningFriction: 0.0 21 | rollingFriction: 0.0 22 | restitution: 0.0 23 | linearDamping: 0.04 24 | angularDamping: 0.04 25 | contactStiffness: 2000.0 26 | contactDamping: 0.7 27 | -------------------------------------------------------------------------------- /rpbi_examples/configs/optas_kuka_lwr_expr2/kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | name: "kuka_lwr" 2 | 3 | loadURDF: 4 | fileName: "{ik_ros_examples}/robots/kuka_lwr.urdf" 5 | useFixedBase: 1 6 | 7 | initial_joint_position: 8 | lwr_arm_0_joint: 0.0 9 | lwr_arm_1_joint: 30.0 10 | lwr_arm_2_joint: 0.0 11 | lwr_arm_3_joint: -90.0 12 | lwr_arm_4_joint: 0.0 13 | lwr_arm_5_joint: -30. 14 | lwr_arm_6_joint: 0.0 15 | 16 | initial_revolute_joint_positions_are_deg: true 17 | is_visual_robot: false 18 | 19 | setJointMotorControlArray: 20 | controlMode: "POSITION_CONTROL" 21 | 22 | publish_joint_state_frequency: 30 23 | 24 | broadcast_link_states: true 25 | broadcast_link_states_frequency: 30 26 | -------------------------------------------------------------------------------- /rpbi_examples/configs/nsl/box.yaml: -------------------------------------------------------------------------------- 1 | name: "pushing_box" 2 | 3 | broadcast_hz: 50 4 | 5 | createVisualShape: 6 | shapeType: "GEOM_BOX" 7 | halfExtents: [0.125, 0.125, 0.125] 8 | rgbaColor: [0.9, 1.0, 0.3, 1.0] 9 | 10 | createCollisionShape: 11 | shapeType: "GEOM_BOX" 12 | # halfExtents: [0.1, 0.1, 0.075] 13 | halfExtents: [0.125, 0.125, 0.125] 14 | 15 | baseMass: 0.5 16 | # basePosition: [-0.6, 0.225, 0.3] 17 | basePosition: [-0.6, 0.25, 0.3] 18 | 19 | changeDynamics: 20 | lateralFriction: 1.0 21 | spinningFriction: 0.0 22 | rollingFriction: 0.0 23 | restitution: 0.0 24 | linearDamping: 0.04 25 | angularDamping: 0.04 26 | contactStiffness: 2000.0 27 | contactDamping: 0.7 28 | -------------------------------------------------------------------------------- /rpbi_examples/launch/basic_example_human_model.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /doc/Makefile: -------------------------------------------------------------------------------- 1 | # Minimal makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line, and also 5 | # from the environment for the first two. 6 | SPHINXOPTS ?= 7 | SPHINXBUILD ?= sphinx-build 8 | SOURCEDIR = . 9 | BUILDDIR = _build 10 | 11 | # Put it first so that "make" without argument is like "make help". 12 | help: 13 | @$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 14 | 15 | .PHONY: help Makefile 16 | 17 | # Catch-all target: route all unknown targets to Sphinx using the new 18 | # "make mode" option. $(O) is meant as a shortcut for $(SPHINXOPTS). 19 | %: Makefile 20 | @$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O) 21 | -------------------------------------------------------------------------------- /rpbi_examples/launch/run_kuka.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /rpbi_examples/configs/interpolation/traj_lwr.yaml: -------------------------------------------------------------------------------- 1 | motion_dimensions: 2 | number: 7 3 | 4 | trans: 5 | translation_x: 6 | translation_x_index: 0 7 | translation_y: 8 | translation_y_index: 1 9 | translation_z: 10 | translation_z_index: 2 11 | 12 | # 'quat' and [3,7] for quaternion, 'euler' and [3,6] for euler angles 13 | rotation: 14 | rotation_repr: 'quat' 15 | rotation_vec: 16 | rotation_angle: 17 | rotation_vec_index: [3, 7] 18 | 19 | 20 | interpolation: 21 | nochange_window_length: 1 22 | use_interpolation: True 23 | inter_dt: 0.01 24 | 25 | communication: 26 | publisher: 27 | header_frame_id: 'rpbi/kuka_lwr/lwr_arm_0_link' 28 | msg_child_frame_id: 'interpol_target' 29 | -------------------------------------------------------------------------------- /rpbi_examples/configs/run_kuka/kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | name: "kuka_lwr" 2 | 3 | loadURDF: 4 | fileName: "{rpbi_examples}/robots/kuka_lwr.urdf" 5 | useFixedBase: 1 6 | 7 | initial_joint_position: 8 | lwr_arm_0_joint: 0.0 9 | lwr_arm_1_joint: 30.0 10 | lwr_arm_2_joint: 0.0 11 | lwr_arm_3_joint: -90.0 12 | lwr_arm_4_joint: 0.0 13 | lwr_arm_5_joint: 60.0 14 | lwr_arm_6_joint: 0.0 15 | 16 | initial_revolute_joint_positions_are_deg: true 17 | is_visual_robot: false 18 | 19 | setJointMotorControlArray: 20 | controlMode: "POSITION_CONTROL" 21 | 22 | enabled_joint_force_torque_sensors: 23 | - "lwr_arm_6_joint" 24 | 25 | publish_joint_state_frequency: 30 26 | 27 | broadcast_link_states: true 28 | brodcast_link_states_frequency: 30 29 | -------------------------------------------------------------------------------- /rpbi_examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(rpbi_examples) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | std_msgs 7 | sensor_msgs 8 | ros_pybullet_interface 9 | ik_ros_examples 10 | ) 11 | 12 | catkin_package() 13 | 14 | include_directories( 15 | ${catkin_INCLUDE_DIRS} 16 | ) 17 | 18 | catkin_install_python(PROGRAMS 19 | scripts/basic_robot_example_node.py 20 | scripts/pybullet_objects_example_node.py 21 | scripts/orbit_node.py 22 | scripts/run_teleop.py 23 | scripts/run_lfd.py 24 | scripts/run_human_interaction.py 25 | scripts/nsl.py 26 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 27 | ) 28 | 29 | install(DIRECTORY configs launch robots DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/) 30 | -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/kinova.yaml: -------------------------------------------------------------------------------- 1 | name: "kinova" 2 | 3 | # 4 | # This example requires the kortex_descrition ROS package. 5 | # https://github.com/Kinovarobotics/ros_kortex/tree/noetic-devel/kortex_description 6 | # 7 | 8 | loadURDF: 9 | fileName: "robot_description" # gets urdf from ros parameter 10 | useFixedBase: 1 11 | 12 | initial_joint_position: 13 | joint_1: -45.0 14 | joint_2: -30 15 | # joint_3: 16 | joint_4: 90. 17 | joint_5: 0 18 | joint_6: 45 19 | joint_7: 90 20 | 21 | initial_revolute_joint_positions_are_deg: true 22 | is_visual_robot: true 23 | 24 | setJointMotorControlArray: 25 | controlMode: "POSITION_CONTROL" 26 | 27 | publish_joint_state_frequency: 30 28 | 29 | broadcast_link_states: true 30 | brodcast_link_states_frequency: 30 31 | -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | name: "kuka_lwr" 2 | 3 | loadURDF: 4 | fileName: "{rpbi_examples}/robots/kuka_lwr.urdf" 5 | useFixedBase: 1 6 | basePosition: [0.0, 0., 0.] 7 | 8 | initial_joint_position: 9 | lwr_arm_0_joint: -45 10 | lwr_arm_1_joint: 30.0 11 | lwr_arm_2_joint: 0.0 12 | lwr_arm_3_joint: -90.0 13 | lwr_arm_4_joint: 0.0 14 | lwr_arm_5_joint: 5.0 15 | lwr_arm_6_joint: 0.0 16 | 17 | initial_revolute_joint_positions_are_deg: true 18 | is_visual_robot: false 19 | 20 | setJointMotorControlArray: 21 | controlMode: "POSITION_CONTROL" 22 | 23 | # enabled_joint_force_torque_sensors: 24 | # - "lwr_arm_6_joint" 25 | 26 | # publish_joint_state_frequency: 30 27 | 28 | # broadcast_link_states: true 29 | # brodcast_link_states_frequency: 30 30 | -------------------------------------------------------------------------------- /rpbi_examples/configs/basic_example_kuka_lwr/kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | name: "kuka_lwr" 2 | 3 | loadURDF: 4 | fileName: "{rpbi_examples}/robots/kuka_lwr.urdf" 5 | useFixedBase: 1 6 | basePosition: [0.5, 0., 0.] 7 | 8 | initial_joint_position: 9 | lwr_arm_0_joint: 0.0 10 | lwr_arm_1_joint: 30.0 11 | lwr_arm_2_joint: 0.0 12 | lwr_arm_3_joint: -90.0 13 | lwr_arm_4_joint: 0.0 14 | lwr_arm_5_joint: 60.0 15 | lwr_arm_6_joint: 0.0 16 | 17 | initial_revolute_joint_positions_are_deg: true 18 | is_visual_robot: false 19 | 20 | setJointMotorControlArray: 21 | controlMode: "POSITION_CONTROL" 22 | 23 | enabled_joint_force_torque_sensors: 24 | - "lwr_arm_6_joint" 25 | 26 | publish_joint_state_frequency: 30 27 | 28 | broadcast_link_states: true 29 | brodcast_link_states_frequency: 30 30 | -------------------------------------------------------------------------------- /rpbi_examples/configs/basic_example_human_model/kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | name: "kuka_lwr" 2 | 3 | loadURDF: 4 | fileName: "{rpbi_examples}/robots/kuka_lwr.urdf" 5 | useFixedBase: 1 6 | basePosition: [0.5, 0., 0.] 7 | 8 | initial_joint_position: 9 | lwr_arm_0_joint: 0.0 10 | lwr_arm_1_joint: 30.0 11 | lwr_arm_2_joint: 0.0 12 | lwr_arm_3_joint: -90.0 13 | lwr_arm_4_joint: 0.0 14 | lwr_arm_5_joint: 60.0 15 | lwr_arm_6_joint: 0.0 16 | 17 | initial_revolute_joint_positions_are_deg: true 18 | is_visual_robot: false 19 | 20 | setJointMotorControlArray: 21 | controlMode: "POSITION_CONTROL" 22 | 23 | enabled_joint_force_torque_sensors: 24 | - "lwr_arm_6_joint" 25 | 26 | publish_joint_state_frequency: 30 27 | 28 | broadcast_link_states: true 29 | brodcast_link_states_frequency: 30 30 | -------------------------------------------------------------------------------- /rpbi_examples/configs/human_interaction/kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | name: "kuka_lwr" 2 | 3 | loadURDF: 4 | fileName: "{ik_ros_examples}/robots/kuka_lwr2.urdf" 5 | useFixedBase: 1 6 | 7 | initial_joint_position: 8 | lwr_arm_0_joint: 0.0 9 | lwr_arm_1_joint: 30.0 10 | lwr_arm_2_joint: 0.0 11 | lwr_arm_3_joint: -90.0 12 | lwr_arm_4_joint: 0.0 13 | lwr_arm_5_joint: 60.0 14 | lwr_arm_6_joint: 0.0 15 | 16 | initial_revolute_joint_positions_are_deg: true 17 | is_visual_robot: false 18 | 19 | setJointMotorControlArray: 20 | controlMode: "POSITION_CONTROL" 21 | 22 | enabled_joint_force_torque_sensors: 23 | - "force_torque_sensor_mount" 24 | 25 | publish_joint_state_frequency: 100 26 | 27 | broadcast_link_states: true 28 | broadcast_link_states_frequency: 30 29 | 30 | start_ik_callback: true 31 | -------------------------------------------------------------------------------- /rpbi_examples/configs/teleop/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 1.0 31 | cameraYaw: 45 32 | cameraPitch: -45.0 33 | cameraTargetPosition: [-0.2, 0.0, 0.3] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | collision_objects: 40 | - "{rpbi_examples}/configs/floor.yaml" 41 | robots: 42 | - "{rpbi_examples}/configs/teleop/kuka_lwr.yaml" 43 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | devel/ 2 | logs/ 3 | build/ 4 | bin/ 5 | lib/ 6 | msg_gen/ 7 | srv_gen/ 8 | msg/*Action.msg 9 | msg/*ActionFeedback.msg 10 | msg/*ActionGoal.msg 11 | msg/*ActionResult.msg 12 | msg/*Feedback.msg 13 | msg/*Goal.msg 14 | msg/*Result.msg 15 | msg/_*.py 16 | build_isolated/ 17 | devel_isolated/ 18 | 19 | # Generated by dynamic reconfigure 20 | *.cfgc 21 | /cfg/cpp/ 22 | /cfg/*.py 23 | 24 | # Ignore generated docs 25 | *.dox 26 | *.wikidoc 27 | 28 | # eclipse stuff 29 | .project 30 | .cproject 31 | 32 | # qcreator stuff 33 | CMakeLists.txt.user 34 | 35 | srv/_*.py 36 | *.pcd 37 | *.pyc 38 | qtcreator-* 39 | *.user 40 | 41 | /planning/cfg 42 | /planning/docs 43 | /planning/src 44 | 45 | *~ 46 | 47 | # Emacs 48 | .#* 49 | 50 | # Catkin custom files 51 | CATKIN_IGNORE 52 | 53 | # python 54 | .idea -------------------------------------------------------------------------------- /rpbi_examples/configs/basic_example_kinova/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 1.4 31 | cameraYaw: 30.0 32 | cameraPitch: -45.0 33 | cameraTargetPosition: [0.0, 0.0, 0.4] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | collision_objects: 40 | - "{rpbi_examples}/configs/floor.yaml" 41 | robots: 42 | - "{rpbi_examples}/configs/basic_example_kinova/kinova.yaml" 43 | -------------------------------------------------------------------------------- /rpbi_examples/configs/nsl/kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | name: "kuka_lwr" 2 | 3 | loadURDF: 4 | fileName: "{ik_ros_examples}/robots/kuka_lwr.urdf" 5 | useFixedBase: 1 6 | 7 | initial_joint_position: 8 | lwr_arm_0_joint: 0.0 9 | lwr_arm_1_joint: 30.0 10 | lwr_arm_2_joint: 0.0 11 | lwr_arm_3_joint: -90.0 12 | lwr_arm_4_joint: 0.0 13 | lwr_arm_5_joint: 60.0 14 | lwr_arm_6_joint: 0.0 15 | 16 | initial_revolute_joint_positions_are_deg: true 17 | is_visual_robot: false 18 | move_to_initial_joint_state_on_startup: true 19 | 20 | setJointMotorControlArray: 21 | controlMode: "POSITION_CONTROL" 22 | 23 | publish_joint_state_frequency: 50 24 | 25 | enabled_joint_force_torque_sensors: 26 | - "force_torque_sensor_mount" 27 | - "lwr_arm_7_joint" 28 | 29 | broadcast_link_states: true 30 | broadcast_link_states_frequency: 30 31 | -------------------------------------------------------------------------------- /rpbi_examples/configs/optas_kuka_lwr_expr/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 1.0 31 | cameraYaw: 45 32 | cameraPitch: -45.0 33 | cameraTargetPosition: [-0.6, 0.0, 0.3] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | robots: 40 | - "{rpbi_examples}/configs/optas_kuka_lwr_expr/kuka_lwr.yaml" 41 | collision_objects: 42 | - "{rpbi_examples}/configs/floor.yaml" 43 | -------------------------------------------------------------------------------- /rpbi_examples/configs/basic_example_talos/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 2.0 31 | cameraYaw: 45 32 | cameraPitch: -45.0 33 | cameraTargetPosition: [0.0, 0.0, 0.75] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | 40 | robots: 41 | - "{rpbi_examples}/configs/basic_example_talos/talos.yaml" 42 | 43 | collision_objects: 44 | - "{rpbi_examples}/configs/floor.yaml" 45 | -------------------------------------------------------------------------------- /rpbi_examples/configs/nsl_haption/kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | name: "kuka_lwr" 2 | 3 | loadURDF: 4 | fileName: "{ik_ros_examples}/robots/kuka_lwr.urdf" 5 | useFixedBase: 1 6 | 7 | initial_joint_position: 8 | lwr_arm_0_joint: 0.0 9 | lwr_arm_1_joint: 30.0 10 | lwr_arm_2_joint: 0.0 11 | lwr_arm_3_joint: -90.0 12 | lwr_arm_4_joint: 0.0 13 | lwr_arm_5_joint: 60.0 14 | lwr_arm_6_joint: 0.0 15 | 16 | initial_revolute_joint_positions_are_deg: true 17 | is_visual_robot: false 18 | move_to_initial_joint_state_on_startup: true 19 | 20 | setJointMotorControlArray: 21 | controlMode: "POSITION_CONTROL" 22 | 23 | publish_joint_state_frequency: 50 24 | 25 | enabled_joint_force_torque_sensors: 26 | - "force_torque_sensor_mount" 27 | - "lwr_arm_7_joint" 28 | 29 | broadcast_link_states: true 30 | broadcast_link_states_frequency: 30 31 | -------------------------------------------------------------------------------- /rpbi_examples/configs/nsl_haption/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | # options: "--opengl2" 12 | 13 | setGravity: 14 | gravX: 0.0 15 | gravY: 0.0 16 | gravZ: -9.81 17 | 18 | timeStep: 0.01 19 | start_pybullet_after_initialization: true 20 | status_hz: 50 21 | 22 | 23 | # 24 | # Pybullet visualizer 25 | # 26 | configureDebugVisualizer: 27 | enable: 0 28 | flag: 'COV_ENABLE_GUI' 29 | 30 | resetDebugVisualizerCamera: 31 | cameraDistance: 1.0 32 | cameraYaw: 45 33 | cameraPitch: -45.0 34 | cameraTargetPosition: [-0.6, 0.0, 0.3] 35 | 36 | 37 | # 38 | # Pybullet objects 39 | # 40 | robots: 41 | - "{rpbi_examples}/configs/nsl_haption/kuka_lwr.yaml" 42 | collision_objects: 43 | - "{rpbi_examples}/configs/floor.yaml" 44 | -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/plane.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /rpbi_examples/configs/basic_example_human_model/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 2.0 31 | cameraYaw: 0.0 32 | cameraPitch: -45.0 33 | cameraTargetPosition: [0.0, 0.0, 0.0] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | collision_objects: 40 | - "{rpbi_examples}/configs/floor.yaml" 41 | robots: 42 | - "{rpbi_examples}/configs/basic_example_human_model/human_model.yaml" 43 | 44 | 45 | -------------------------------------------------------------------------------- /rpbi_examples/configs/lfd/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 1.0 31 | cameraYaw: 45 32 | cameraPitch: -45.0 33 | cameraTargetPosition: [-0.6, 0.0, 0.3] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | visual_objects: 40 | - "{rpbi_examples}/configs/lfd/visual_goal.yaml" 41 | collision_objects: 42 | - "{rpbi_examples}/configs/floor.yaml" 43 | robots: 44 | - "{rpbi_examples}/configs/lfd/kuka_lwr.yaml" 45 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_visual_object.py: -------------------------------------------------------------------------------- 1 | from .pybullet_object import PybulletObject 2 | from .pybullet_object_pose import PybulletObjectPose 3 | 4 | class PybulletVisualObject(PybulletObject): 5 | 6 | """Simply visualizes an object in Pybullet.""" 7 | 8 | def init(self): 9 | 10 | # Get visual shape index 11 | self.base_visual_shape_index = self.create_visual_shape(self.createVisualShape) 12 | 13 | # Setup body unique id 14 | self.body_unique_id = self.pb.createMultiBody(baseVisualShapeIndex=self.base_visual_shape_index) 15 | 16 | # Setup object pose handler 17 | self.pose = PybulletObjectPose(self) 18 | if self.pose.tf_specified(): 19 | self.pose.start_reset_pose() 20 | 21 | @property 22 | def createVisualShape(self): 23 | return self.config['createVisualShape'] 24 | -------------------------------------------------------------------------------- /rpbi_examples/configs/optas_kuka_lwr_expr2/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | # options: '--mp4=$HOME/optas_kuka_lwr_expr2.mp4' 12 | 13 | setGravity: 14 | gravX: 0.0 15 | gravY: 0.0 16 | gravZ: -9.81 17 | 18 | timeStep: 0.01 19 | start_pybullet_after_initialization: true 20 | status_hz: 50 21 | 22 | 23 | # 24 | # Pybullet visualizer 25 | # 26 | configureDebugVisualizer: 27 | enable: 0 28 | flag: 'COV_ENABLE_GUI' 29 | 30 | resetDebugVisualizerCamera: 31 | cameraDistance: 1.0 32 | cameraYaw: 45 33 | cameraPitch: -45.0 34 | cameraTargetPosition: [-0.6, 0.0, 0.3] 35 | 36 | 37 | # 38 | # Pybullet objects 39 | # 40 | robots: 41 | - "{rpbi_examples}/configs/optas_kuka_lwr_expr2/kuka_lwr.yaml" 42 | collision_objects: 43 | - "{rpbi_examples}/configs/floor.yaml" 44 | -------------------------------------------------------------------------------- /rpbi_examples/configs/run_kuka/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | # options: "--mp4=$HOME/fig8.mp4" 12 | 13 | setGravity: 14 | gravX: 0.0 15 | gravY: 0.0 16 | gravZ: -9.81 17 | 18 | timeStep: 0.01 19 | start_pybullet_after_initialization: true 20 | status_hz: 50 21 | 22 | 23 | # 24 | # Pybullet visualizer 25 | # 26 | configureDebugVisualizer: 27 | enable: 0 28 | flag: 'COV_ENABLE_GUI' 29 | 30 | resetDebugVisualizerCamera: 31 | cameraDistance: 2.0 32 | cameraYaw: 0.0 33 | cameraPitch: -45.0 34 | cameraTargetPosition: [0.0, 0.0, 0.0] 35 | 36 | 37 | # 38 | # Pybullet objects 39 | # 40 | collision_objects: 41 | - "{rpbi_examples}/configs/floor.yaml" 42 | robots: 43 | - "{rpbi_examples}/configs/run_kuka/kuka_lwr.yaml" 44 | 45 | 46 | controls: 47 | robot_name: "kuka" 48 | -------------------------------------------------------------------------------- /rpbi_examples/configs/human_interaction/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 1.0 31 | cameraYaw: 0 32 | cameraPitch: -0 33 | cameraTargetPosition: [-0.5322363630, 0.0245886579, 0.2402224259] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | collision_objects: 40 | - "{rpbi_examples}/configs/floor.yaml" 41 | - "{rpbi_examples}/configs/human_interaction/box.yaml" 42 | robots: 43 | - "{rpbi_examples}/configs/human_interaction/kuka_lwr.yaml" 44 | -------------------------------------------------------------------------------- /doc/cite.rst: -------------------------------------------------------------------------------- 1 | Cite 2 | ==== 3 | 4 | If you use the ROS-PyBullet Interface in your work, please consider citing us using the following. 5 | 6 | .. code-block:: bibtex 7 | 8 | @misc{https://doi.org/10.48550/arxiv.2210.06887, 9 | doi = {10.48550/ARXIV.2210.06887}, 10 | url = {https://arxiv.org/abs/2210.06887}, 11 | author = {Mower, Christopher E. and Stouraitis, Theodoros and Moura, João and Rauch, Christian and Yan, Lei and Behabadi, Nazanin Zamani and Gienger, Michael and Vercauteren, Tom and Bergeles, Christos and Vijayakumar, Sethu}, 12 | keywords = {Robotics (cs.RO), Machine Learning (cs.LG), FOS: Computer and information sciences, FOS: Computer and information sciences}, 13 | title = {ROS-PyBullet Interface: A Framework for Reliable Contact Simulation and Human-Robot Interaction}, 14 | publisher = {arXiv}, 15 | year = {2022}, 16 | copyright = {Creative Commons Attribution 4.0 International} 17 | } 18 | -------------------------------------------------------------------------------- /rpbi_examples/configs/nsl/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | # options: "--opengl2" 12 | 13 | setGravity: 14 | gravX: 0.0 15 | gravY: 0.0 16 | gravZ: -9.81 17 | 18 | timeStep: 0.01 19 | start_pybullet_after_initialization: true 20 | status_hz: 50 21 | 22 | enable_keyboard_publisher: true 23 | 24 | 25 | # 26 | # Pybullet visualizer 27 | # 28 | configureDebugVisualizer: 29 | enable: 0 30 | flag: 'COV_ENABLE_GUI' 31 | 32 | resetDebugVisualizerCamera: 33 | cameraDistance: 1.0 34 | cameraYaw: 45 35 | cameraPitch: -45.0 36 | cameraTargetPosition: [-0.6, 0.0, 0.3] 37 | 38 | 39 | # 40 | # Pybullet objects 41 | # 42 | robots: 43 | - "{rpbi_examples}/configs/nsl/kuka_lwr.yaml" 44 | collision_objects: 45 | - "{rpbi_examples}/configs/floor.yaml" 46 | visual_objects: 47 | - "{rpbi_examples}/configs/nsl/goal_box.yaml" 48 | -------------------------------------------------------------------------------- /doc/make.bat: -------------------------------------------------------------------------------- 1 | @ECHO OFF 2 | 3 | pushd %~dp0 4 | 5 | REM Command file for Sphinx documentation 6 | 7 | if "%SPHINXBUILD%" == "" ( 8 | set SPHINXBUILD=sphinx-build 9 | ) 10 | set SOURCEDIR=. 11 | set BUILDDIR=_build 12 | 13 | %SPHINXBUILD% >NUL 2>NUL 14 | if errorlevel 9009 ( 15 | echo. 16 | echo.The 'sphinx-build' command was not found. Make sure you have Sphinx 17 | echo.installed, then set the SPHINXBUILD environment variable to point 18 | echo.to the full path of the 'sphinx-build' executable. Alternatively you 19 | echo.may add the Sphinx directory to PATH. 20 | echo. 21 | echo.If you don't have Sphinx installed, grab it from 22 | echo.https://www.sphinx-doc.org/ 23 | exit /b 1 24 | ) 25 | 26 | if "%1" == "" goto help 27 | 28 | %SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 29 | goto end 30 | 31 | :help 32 | %SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O% 33 | 34 | :end 35 | popd 36 | -------------------------------------------------------------------------------- /ros_pybullet_interface/msg/PybulletObject.msg: -------------------------------------------------------------------------------- 1 | # object types 2 | uint8 VISUAL=0 3 | uint8 COLLISION=1 4 | uint8 DYNAMIC=2 5 | uint8 ROBOT=3 6 | uint8 SOFT=4 7 | uint8 URDF=5 8 | 9 | uint8 object_type # use above 10 | 11 | # Configuration filename 12 | # NOTE: if filename is specified then config is ignored 13 | string filename 14 | 15 | # Configuration as a string 16 | # NOTE: this is used when filename is not given 17 | # 18 | # If you want to use this then you need to send the configuration as a 19 | # string that is ultimately a yaml file. The best way to do this is is 20 | # to use the config_to_str method provided in 21 | # custom_ros_tools.config. See example below. 22 | # 23 | # from custom_ros_tools.config import config_to_str 24 | # from ros_pybullet_interface.msg import PybulletObject 25 | # 26 | # # make config 27 | # config = {} 28 | # # ... 29 | # 30 | # req = PybulletObject(config=config_to_str(config)) 31 | # 32 | string config 33 | -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/cube.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /rpbi_examples/launch/pybullet_objects_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /rpbi_utils/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | rpbi_utils 7 | 2.0.0 8 | The rpbi_utils package 9 | Christopher E. Mower 10 | BSD 11 | https://github.com/cmower/ros_pybullet_interface 12 | Christopher E. Mower 13 | Theodoros Stouraitis 14 | Lei Yan 15 | Joao Moura 16 | catkin 17 | rospy 18 | tf2 19 | tf2_ros 20 | python-numpy 21 | python3-scipy 22 | geometry_msgs 23 | std_msgs 24 | 25 | -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/config_talos.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 2.0 31 | cameraYaw: 45 32 | cameraPitch: -45.0 33 | cameraTargetPosition: [0.0, 0.0, 0.75] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | 40 | robots: 41 | - "{rpbi_examples}/configs/run_pose/talos.yaml" 42 | # - "{rpbi_examples}/configs/run_pose/kuka_lwr.yaml" 43 | # - "{rpbi_examples}/configs/run_pose/human_model.yaml" 44 | # - "{rpbi_examples}/configs/run_pose/kinova.yaml" 45 | 46 | collision_objects: 47 | - "{rpbi_examples}/configs/floor.yaml" 48 | -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/config_human_model.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 3 31 | cameraYaw: 80 32 | cameraPitch: -30.0 33 | cameraTargetPosition: [0.0, 0.0, 0.35] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | 40 | robots: 41 | # - "{rpbi_examples}/configs/run_pose/talos.yaml" 42 | # - "{rpbi_examples}/configs/run_pose/kuka_lwr.yaml" 43 | - "{rpbi_examples}/configs/run_pose/human_model.yaml" 44 | # - "{rpbi_examples}/configs/run_pose/kinova.yaml" 45 | 46 | collision_objects: 47 | - "{rpbi_examples}/configs/floor.yaml" 48 | -------------------------------------------------------------------------------- /rpbi_examples/configs/interpolation/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 1.0 31 | cameraYaw: -45 32 | cameraPitch: -45.0 33 | cameraTargetPosition: [-0.6, 0.0, 0.3] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | visual_objects: 40 | - "{rpbi_examples}/configs/interpolation/visual_wpt1.yaml" 41 | - "{rpbi_examples}/configs/interpolation/visual_wpt2.yaml" 42 | - "{rpbi_examples}/configs/interpolation/visual_wpt3.yaml" 43 | collision_objects: 44 | - "{rpbi_examples}/configs/floor.yaml" 45 | robots: 46 | - "{rpbi_examples}/configs/interpolation/kuka_lwr.yaml" 47 | -------------------------------------------------------------------------------- /rpbi_examples/launch/pybullet_rgbd_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /rpbi_examples/launch/basic_example_kuka_lwr.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /ros_pybullet_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ros_pybullet_interface) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | sensor_msgs 7 | geometry_msgs 8 | cob_srvs 9 | message_generation 10 | ) 11 | 12 | catkin_python_setup() 13 | 14 | add_message_files( 15 | FILES 16 | ResetDebugVisualizerCamera.msg 17 | JointInfo.msg 18 | CalculateInverseKinematicsProblem.msg 19 | PybulletObject.msg 20 | KeyboardEvent.msg 21 | MouseEvent.msg 22 | ) 23 | 24 | add_service_files( 25 | FILES 26 | RobotInfo.srv 27 | ResetJointState.srv 28 | ResetEffState.srv 29 | CalculateInverseKinematics.srv 30 | GetDebugVisualizerCamera.srv 31 | AddPybulletObject.srv 32 | ) 33 | 34 | generate_messages( 35 | DEPENDENCIES 36 | std_msgs 37 | geometry_msgs 38 | sensor_msgs 39 | ) 40 | 41 | catkin_package() 42 | 43 | include_directories( 44 | ${catkin_INCLUDE_DIRS} 45 | ) 46 | 47 | catkin_install_python(PROGRAMS 48 | scripts/ros_pybullet_interface_node.py 49 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 50 | ) 51 | -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/config_kuka_lwr.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 2.0 31 | cameraYaw: 45 32 | cameraPitch: -45.0 33 | cameraTargetPosition: [0.0, 0.0, 0.75] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | 40 | robots: 41 | # - "{rpbi_examples}/configs/run_pose/talos.yaml" 42 | - "{rpbi_examples}/configs/run_pose/kuka_lwr.yaml" 43 | # - "{rpbi_examples}/configs/run_pose/human_model.yaml" 44 | # - "{rpbi_examples}/configs/run_pose/kinova.yaml" 45 | 46 | # collision_objects: 47 | # - "{rpbi_examples}/configs/floor.yaml" 48 | 49 | visual_objects: 50 | - "{rpbi_examples}/configs/run_pose/whiteboard.yaml" 51 | -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/cube.obj: -------------------------------------------------------------------------------- 1 | # cube.obj 2 | # 3 | 4 | o cube 5 | mtllib cube.mtl 6 | 7 | v -0.500000 -0.500000 0.500000 8 | v 0.500000 -0.500000 0.500000 9 | v -0.500000 0.500000 0.500000 10 | v 0.500000 0.500000 0.500000 11 | v -0.500000 0.500000 -0.500000 12 | v 0.500000 0.500000 -0.500000 13 | v -0.500000 -0.500000 -0.500000 14 | v 0.500000 -0.500000 -0.500000 15 | 16 | vt 0.000000 0.000000 17 | vt 1.000000 0.000000 18 | vt 0.000000 1.000000 19 | vt 1.000000 1.000000 20 | 21 | vn 0.000000 0.000000 1.000000 22 | vn 0.000000 1.000000 0.000000 23 | vn 0.000000 0.000000 -1.000000 24 | vn 0.000000 -1.000000 0.000000 25 | vn 1.000000 0.000000 0.000000 26 | vn -1.000000 0.000000 0.000000 27 | 28 | g cube 29 | usemtl cube 30 | s 1 31 | f 1/1/1 2/2/1 3/3/1 32 | f 3/3/1 2/2/1 4/4/1 33 | s 2 34 | f 3/1/2 4/2/2 5/3/2 35 | f 5/3/2 4/2/2 6/4/2 36 | s 3 37 | f 5/4/3 6/3/3 7/2/3 38 | f 7/2/3 6/3/3 8/1/3 39 | s 4 40 | f 7/1/4 8/2/4 1/3/4 41 | f 1/3/4 8/2/4 2/4/4 42 | s 5 43 | f 2/1/5 8/2/5 4/3/5 44 | f 4/3/5 8/2/5 6/4/5 45 | s 6 46 | f 7/1/6 1/2/6 5/3/6 47 | f 5/3/6 1/2/6 3/4/6 48 | 49 | -------------------------------------------------------------------------------- /rpbi_examples/configs/pybullet_objects_example/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 2.0 31 | cameraYaw: 45.0 32 | cameraPitch: -45.0 33 | cameraTargetPosition: [0.0, 0.0, 0.0] 34 | 35 | 36 | # 37 | # Sensors 38 | # 39 | 40 | rgbd_sensor: 41 | name: 'rgbd_sensor' 42 | hz: 30 43 | intrinsics: 44 | width: 640 45 | height: 480 46 | fov: 40 47 | range: [0.01, 10000] 48 | object_tf: 49 | tf_id: 'rpbi/camera' 50 | 51 | 52 | # 53 | # Pybullet objects 54 | # 55 | visual_objects: 56 | - "{rpbi_examples}/configs/pybullet_objects_example/visual_sphere.yaml" 57 | collision_objects: 58 | - "{rpbi_examples}/configs/floor.yaml" 59 | -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/cup.urdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /doc/develop.rst: -------------------------------------------------------------------------------- 1 | .. _develop: 2 | 3 | Development 4 | =========== 5 | 6 | Contributing 7 | ------------ 8 | 9 | We are more than happy to accept bug fixes, new features, suggestions, comments, or any other form of feedback. 10 | If you have an issue using the interface, or would like a new feature added please `submit an issue `_. 11 | For pull requests, please `fork the repository `_, create a new branch, and submit your pull request. 12 | 13 | Future work 14 | ----------- 15 | 16 | * Port GUI controls to RQT. 17 | * Add additional features to GUI interface, e.g. move to custom joint states. 18 | * Support loading from SDF and MuJoCo configuration files. 19 | * Update ROS communication features for loading objects from URDF configuration files. 20 | 21 | Known issues 22 | ------------ 23 | 24 | * Objects with alpha color values less than 1.0 are rendedered in RGB images but not the depth image for the RGBD sensor simulation. To make sure the depth image contains the object, ensure the alpha value is set to 1.0. 25 | -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/config_kinova.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 2.0 31 | cameraYaw: 45 32 | cameraPitch: -45.0 33 | cameraTargetPosition: [0.0, 0.0, 0.75] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | 40 | robots: 41 | # - "{rpbi_examples}/configs/run_pose/talos.yaml" 42 | # - "{rpbi_examples}/configs/run_pose/kuka_lwr.yaml" 43 | # - "{rpbi_examples}/configs/run_pose/human_model.yaml" 44 | - "{rpbi_examples}/configs/run_pose/kinova.yaml" 45 | 46 | # collision_objects: 47 | # - "{rpbi_examples}/configs/floor.yaml" 48 | 49 | visual_objects: 50 | - "{rpbi_examples}/configs/run_pose/table.yaml" 51 | 52 | urdfs: 53 | - "{rpbi_examples}/configs/run_pose/cup.yaml" 54 | # - "{rpbi_examples}/configs/run_pose/mug.yaml" 55 | -------------------------------------------------------------------------------- /rpbi_examples/configs/basic_example_kuka_lwr/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | options: '--mp4=$HOME/basic_example_kuka_lwr.mp4' 12 | 13 | setGravity: 14 | gravX: 0.0 15 | gravY: 0.0 16 | gravZ: -9.81 17 | 18 | timeStep: 0.01 19 | start_pybullet_after_initialization: true 20 | status_hz: 50 21 | 22 | 23 | # 24 | # Pybullet visualizer 25 | # 26 | configureDebugVisualizer: 27 | enable: 0 28 | flag: 'COV_ENABLE_GUI' 29 | 30 | resetDebugVisualizerCamera: 31 | cameraDistance: 2.0 32 | cameraYaw: 0.0 33 | cameraPitch: -45.0 34 | cameraTargetPosition: [0.0, 0.0, 0.0] 35 | 36 | 37 | # 38 | # Pybullet objects 39 | # 40 | collision_objects: 41 | - "{rpbi_examples}/configs/floor.yaml" 42 | robots: 43 | - "{rpbi_examples}/configs/basic_example_kuka_lwr/kuka_lwr.yaml" 44 | 45 | 46 | # 47 | # Sensors 48 | # 49 | 50 | rgbd_sensor: 51 | name: 'rgbd_sensor' 52 | hz: 30 53 | # this will project the depth to a point cloud 54 | # pointcloud: True 55 | intrinsics: 56 | width: 640 57 | height: 480 58 | fov: 40 59 | range: [0.01, 10000] 60 | object_tf: 61 | tf_id: 'rpbi/camera' 62 | -------------------------------------------------------------------------------- /rpbi_examples/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | rpbi_examples 7 | 2.0.0 8 | The rpbi_examples package 9 | Christopher E. Mower 10 | BSD 11 | https://github.com/cmower/ros_pybullet_interface 12 | Christopher E. Mower 13 | Theodoros Stouraitis 14 | Lei Yan 15 | Joao Moura 16 | BSD 17 | catkin 18 | rospy 19 | rospy 20 | rospy 21 | ik_ros 22 | operator_node 23 | ik_ros_examples 24 | std_msgs 25 | cob_srvs 26 | sensor_msgs 27 | ros_pybullet_interface 28 | keyboard 29 | 30 | -------------------------------------------------------------------------------- /ros_pybullet_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | ros_pybullet_interface 7 | 2.0.0 8 | The ros_pybullet_interface package 9 | Christopher E. Mower 10 | BSD 11 | https://github.com/cmower/ros_pybullet_interface 12 | Christopher E. Mower 13 | Theodoros Stouraitis 14 | Lei Yan 15 | Joao Moura 16 | catkin 17 | rospy 18 | sensor_msgs 19 | geometry_msgs 20 | std_msgs 21 | tf_conversions 22 | std_srvs 23 | cob_srvs 24 | python3-pybullet-pip 25 | python3-numpy 26 | custom_ros_tools 27 | message_generation 28 | message_runtime 29 | 30 | -------------------------------------------------------------------------------- /rpbi_examples/configs/basic_example_nextage/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | 12 | setGravity: 13 | gravX: 0.0 14 | gravY: 0.0 15 | gravZ: -9.81 16 | 17 | timeStep: 0.01 18 | start_pybullet_after_initialization: true 19 | status_hz: 50 20 | 21 | 22 | # 23 | # Pybullet visualizer 24 | # 25 | configureDebugVisualizer: 26 | enable: 0 27 | flag: 'COV_ENABLE_GUI' 28 | 29 | resetDebugVisualizerCamera: 30 | cameraDistance: 2.0 31 | cameraYaw: 0.0 32 | cameraPitch: -45.0 33 | cameraTargetPosition: [0.0, 0.0, 0.0] 34 | 35 | 36 | # 37 | # Pybullet objects 38 | # 39 | collision_objects: 40 | - "{rpbi_examples}/configs/floor.yaml" 41 | # visual_objects: 42 | # - "{rpbi_examples}/configs/basic_example_nextage/table.yaml" 43 | robots: 44 | - "{rpbi_examples}/configs/basic_example_nextage/nextage.yaml" 45 | 46 | 47 | # 48 | # Sensors 49 | # 50 | 51 | # rgbd_sensor: 52 | # name: 'rgbd_sensor' 53 | # hz: 30 54 | # # this will project the depth to a point cloud 55 | # # pointcloud: True 56 | # intrinsics: 57 | # width: 640 58 | # height: 480 59 | # fov: 40 60 | # range: [0.01, 10000] 61 | # object_tf: 62 | # tf_id: 'rpbi/camera' 63 | -------------------------------------------------------------------------------- /rpbi_examples/launch/basic_example_nextage.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /rpbi_examples/configs/soft_body/config.yaml: -------------------------------------------------------------------------------- 1 | # 2 | # Basic example 3 | # 4 | 5 | 6 | # 7 | # Pybullet instance 8 | # 9 | connect: 10 | connection_mode: 'GUI' 11 | # options: "--mp4=$HOME/fig8.mp4" 12 | 13 | setAdditionalSearchPath: 'pybullet_data_path' 14 | 15 | resetSimulation: 16 | flags: "RESET_USE_DEFORMABLE_WORLD" 17 | 18 | setPhysicsEngineParameter: 19 | sparseSdfVoxelSize: 0.25 20 | 21 | setGravity: 22 | gravX: 0.0 23 | gravY: 0.0 24 | gravZ: -10 25 | 26 | timeStep: 0.01 27 | start_pybullet_after_initialization: true 28 | status_hz: 50 29 | 30 | step_pybullet_manually: True 31 | 32 | 33 | # 34 | # Pybullet visualizer 35 | # 36 | configureDebugVisualizer: 37 | enable: 0 38 | flag: 'COV_ENABLE_GUI' 39 | 40 | resetDebugVisualizerCamera: 41 | cameraDistance: 3 42 | cameraYaw: -420 43 | cameraPitch: -30 44 | cameraTargetPosition: [0.3, 0.8, 0.7] 45 | 46 | 47 | # 48 | # Pybullet objects 49 | # 50 | 51 | urdfs: 52 | - "{rpbi_examples}/configs/soft_body/plane.yaml" 53 | - "{rpbi_examples}/configs/soft_body/cube.yaml" 54 | # - "{rpbi_examples}/configs/soft_body/torus_deform.yaml" 55 | # collision_objects: 56 | # - "{rpbi_examples}/configs/soft_body/floor.yaml" 57 | soft_objects: 58 | - "{rpbi_examples}/configs/soft_body/torus.yaml" 59 | 60 | 61 | 62 | controls: 63 | robot_name: "kuka" 64 | -------------------------------------------------------------------------------- /rpbi_examples/launch/nsl_haption.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_collision_object.py: -------------------------------------------------------------------------------- 1 | from .pybullet_object import PybulletObject 2 | from .pybullet_object_pose import PybulletObjectPose 3 | 4 | 5 | class PybulletCollisionObject(PybulletObject): 6 | 7 | """Visualizes an object. Other objects will react to this, but it will remain unaffected.""" 8 | 9 | def init(self): 10 | 11 | # Get visual and collision shape indices 12 | self.base_visual_shape_index = self.create_visual_shape(self.createVisualShape) 13 | self.base_collision_shape_index = self.create_collision_shape(self.createCollisionShape) 14 | 15 | # Create body unique id 16 | self.body_unique_id = self.pb.createMultiBody( 17 | baseMass=0.0, 18 | baseVisualShapeIndex=self.base_visual_shape_index, 19 | baseCollisionShapeIndex=self.base_collision_shape_index, 20 | ) 21 | 22 | # Set dynamics 23 | if self.changeDynamics: 24 | self.change_dynamics(self.changeDynamics) 25 | 26 | # Setup object pose handler 27 | self.pose = PybulletObjectPose(self) 28 | if self.pose.tf_specified(): 29 | self.pose.start_reset_pose() 30 | 31 | @property 32 | def createVisualShape(self): 33 | return self.config['createVisualShape'] 34 | 35 | @property 36 | def createCollisionShape(self): 37 | return self.config['createCollisionShape'] 38 | 39 | @property 40 | def changeDynamics(self): 41 | return self.config.get('changeDynamics') 42 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2022, University of Edinburgh 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | 7 | * Redistributions of source code must retain the above copyright notice, 8 | this list of conditions and the following disclaimer. 9 | * Redistributions in binary form must reproduce the above copyright 10 | notice, this list of conditions and the following disclaimer in the 11 | documentation and/or other materials provided with the distribution. 12 | * Neither the name of nor the names of its contributors may be used to 13 | endorse or promote products derived from this software without specific 14 | prior written permission. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 19 | ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 20 | LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 21 | CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 22 | SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 23 | INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 24 | CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 25 | ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 26 | POSSIBILITY OF SUCH DAMAGE. -------------------------------------------------------------------------------- /rpbi_examples/launch/run_pose_kinova.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /rpbi_examples/launch/basic_example_kinova.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /rpbi_examples/launch/nsl.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /doc/installation.rst: -------------------------------------------------------------------------------- 1 | Installation 2 | ============ 3 | 4 | The following describes how to install the ROS-PyBullet Interface. 5 | 6 | Requirements 7 | ############ 8 | 9 | 10 | * ROS Noetic 11 | 12 | * Melodic, and past version should work fine (only Melodic has been tested) so long as you configure ROS to use Python 3 13 | * ROS2 is currently unsupported, however this is in-development 14 | * `catkin_tools `_ 15 | * `rosinstall `_ 16 | * `rosdep `_ 17 | 18 | * Ubuntu 20.04 19 | 20 | * Other versions should work, provided Python 3 is installed 21 | 22 | * Python 3 23 | 24 | * ``urdf_parser_py``, see `here `_. 25 | 26 | 27 | From binaries 28 | ############# 29 | 30 | Currently, this is *in-progress*. 31 | 32 | From source 33 | ########### 34 | 35 | 1. `Create a catkin workspace `_ or use an existing workspace. `catkin_tools `_ is the preferred build system. 36 | 2. ``cd`` to the ``src`` directory of your catkin workspace. 37 | 3. Clone this repository: ``$ git clone https://github.com/cmower/ros_pybullet_interface.git`` 38 | 4. Install source dependencies: ``$ rosinstall . --catkin --nobuild`` 39 | 5. Install binary dependencies: ``$ rosdep update ; rosdep install --from-paths ./ -iry`` 40 | 6. Compile the workspace: ``$ catkin build -s`` 41 | 7. Source the workspace: ``$ source $(catkin locate)/devel/setup.bash`` 42 | 43 | Now you should be able to run the :ref:`examples `. 44 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS PyBullet Interface 2 | 3 | **NOTE: This version of the repository is not supported anymore, all future development will be [here](https://github.com/ros-pybullet/ros_pybullet_interface). Please raise any issues [here](https://github.com/ros-pybullet/ros_pybullet_interface/issues).** 4 | 5 | The ROS-PyBullet Interface is a framework that provides a bridge between the popular Robot Operating System (ROS) with a reliable impact/contact simulator PyBullet. 6 | Furthermore, this framework provides several interfaces that allow humans to interact with the simulator that facilitates Human-Robot Interaction in a virtual world. 7 | 8 | # Requirements 9 | 10 | * ROS Noetic 11 | * Ubuntu 20.04 12 | * Python 3 13 | 14 | # Install 15 | 16 | 1. [Create a catkin workspace.](https://catkin-tools.readthedocs.io/en/latest/quick_start.html#initializing-a-new-workspace) 17 | 2. `cd` into the `src` directory. 18 | 3. Clone this repository: `git clone https://github.com/ros-pybullet/ros_pybullet_interface.git` 19 | 4. `cd ros_pybullet_interface` 20 | 5. Inspect `install.sh`, and if you are happy then run: `bash install.sh` 21 | 22 | Now, you should be able to run the examples. 23 | 24 | # Cite 25 | 26 | If you use the interface in your work, please consider citing us. 27 | 28 | ```bibtex 29 | @article{Mower2022, 30 | author = {Mower, Christopher E. and Stouraitis, Theodoros and Moura, João and Rauch, Christian and Yan, Lei and Behabadi, Nazanin Zamani and Gienger, Michael and Vercauteren, Tom and Bergeles, Christos and Vijayakumar, Sethu}, 31 | title = {ROS-PyBullet Interface: A Framework for Reliable Contact Simulation and Human-Robot Interaction}, 32 | journal = {[to appear] Proceedings of the Conference on Robot Learning}, 33 | year = {2022}, 34 | } 35 | ``` 36 | -------------------------------------------------------------------------------- /rpbi_utils/scripts/calibrate_wrench_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import rospy 3 | import numpy as np 4 | from geometry_msgs.msg import WrenchStamped 5 | 6 | 7 | class Node: 8 | 9 | 10 | def __init__(self): 11 | rospy.init_node('calibrate_wrench_node', anonymous=True) 12 | self._max_samples = rospy.get_param('~max_samples', 100) 13 | self._samples = [] 14 | self._offset = None 15 | self._pub = rospy.Publisher('wrench/calibrated', WrenchStamped, queue_size=10) 16 | self._sub = rospy.Subscriber('wrench', WrenchStamped, self._callback) 17 | 18 | 19 | def _callback(self, msg): 20 | 21 | n_samples = len(self._samples) 22 | 23 | if n_samples < self._max_samples: 24 | self._samples.append(msg.wrench) 25 | 26 | elif (n_samples == self._max_samples) and self._offset is None: 27 | 28 | offset = np.array([ 29 | [s.force.x, 30 | s.force.y, 31 | s.force.z, 32 | s.torque.x, 33 | s.torque.y, 34 | s.torque.z] 35 | for s in self._samples 36 | ]) 37 | 38 | self._offset = offset.mean(axis=0) 39 | 40 | else: 41 | 42 | msg.wrench.force.x -= self._offset[0] 43 | msg.wrench.force.y -= self._offset[1] 44 | msg.wrench.force.z -= self._offset[2] 45 | msg.wrench.torque.x -= self._offset[3] 46 | msg.wrench.torque.y -= self._offset[4] 47 | msg.wrench.torque.z -= self._offset[5] 48 | 49 | self._pub.publish(msg) 50 | 51 | 52 | def spin(self): 53 | rospy.spin() 54 | 55 | 56 | def main(): 57 | Node().spin() 58 | 59 | 60 | if __name__ == '__main__': 61 | main() 62 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_robot_links.py: -------------------------------------------------------------------------------- 1 | 2 | class Links: 3 | 4 | def __init__(self, pb_obj, joints, urdf): 5 | self.pb_obj = pb_obj 6 | self.urdf = urdf 7 | self.joints = joints 8 | self.root_link_name = self.get_root_link_name() # HACK: see method below 9 | 10 | if self.broadcast_link_states: 11 | dt = self.pb_obj.node.Duration(1.0/float(self.broadcast_link_states_hz)) 12 | self.pb_obj.timers['publish_link_states'] = self.pb_obj.node.Timer(dt, self._broadcast_link_states) 13 | 14 | def _broadcast_link_states(self, event): 15 | 16 | # Get base position/orientation for root link 17 | pos, rot = self.pb_obj.pb.getBasePositionAndOrientation(self.pb_obj.body_unique_id) 18 | self.pb_obj.node.tf.set_tf('rpbi/world', f'rpbi/{self.pb_obj.name}/{self.root_link_name}', pos, rot) 19 | 20 | # Iterate over joints 21 | link_states = self.pb_obj.pb.getLinkStates(self.pb_obj.body_unique_id, self.joints.indices, computeForwardKinematics=1) 22 | for joint, link_state in zip(self.joints, link_states): 23 | self.pb_obj.node.tf.set_tf('rpbi/world', f'rpbi/{self.pb_obj.name}/{joint.linkName}', link_state[4], link_state[5]) 24 | 25 | @property 26 | def broadcast_link_states(self): 27 | return self.pb_obj.config.get('broadcast_link_states', False) 28 | 29 | @property 30 | def broadcast_link_states_hz(self): 31 | return self.pb_obj.config.get('broadcast_link_states_hz', 50) 32 | 33 | def get_root_link_name(self): 34 | """Return the root link name.""" 35 | # HACK: since I haven't been able to find how to retrieve the root link name from pybullet, I had to use urdf_parser_py instead 36 | from urdf_parser_py import urdf 37 | with open(self.urdf.filename, 'r') as f: 38 | robot = urdf.Robot.from_xml_string(f.read()) 39 | return robot.get_root() 40 | -------------------------------------------------------------------------------- /doc/overview.rst: -------------------------------------------------------------------------------- 1 | Overview 2 | ======== 3 | 4 | Reliable contact simulation is a key requirement for the community developing (semi-)autonomous robots. 5 | Simulation, data collection, and straight-forward interfacing with robot hardware and multi-modal sensing will enable the development of robust machine learning algorithms and control approaches for real world applications. 6 | The ROS-PyBullet Interface is a framework that provides a bridge between the popular Robot Operating System (ROS) with a reliable impact/contact simulator Pybullet. 7 | Furthermore, this framework provides several interfaces that allow humans to interact with the simulator that facilitates Human-Robot Interaction in a virtual world. 8 | Tutorial slides found `here `_. 9 | 10 | The main features of the framework is summarized as follows. 11 | 12 | 1. Online, full-physics simulation of robots using a reliable simulator. The framework uses Pybullet to enable well founded contact simulation. 13 | 2. Integration with ROS ecosystem. Tracking the state of the world using Pybullet is integrated with ROS so that users can easily plugin their work in a similar way that a real system might be controlled. 14 | 3. Several interfaces for humans. Providing demonstrations from humans requires an interface to the simulated environment. In addition, utilizing a haptic interface the human can directly interact with the virtual world. 15 | 4. Modular and extensible design. Our proposed framework adopts a modular and highly extensible design paradigm using Python. This makes it easy for practitioners to develop and prototype their methods for several tasks. 16 | 5. Data collection with standard ROS tools. Since the framework provides an interface to ROS we can use common tools for data collection such as ROS bags and ``rosbag_pandas``. 17 | 6. Easily integrates with hardware. Tools are provided to easily remap the virtual system to physical hardware. 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /rpbi_examples/scripts/pybullet_objects_example_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | from rpbi.ros_node import RosNode 3 | from cob_srvs.srv import SetString, SetStringRequest 4 | from ros_pybullet_interface.msg import PybulletObject 5 | from ros_pybullet_interface.srv import AddPybulletObject 6 | 7 | 8 | class Node(RosNode): 9 | 10 | 11 | interval = 3.0 12 | 13 | 14 | def __init__(self): 15 | RosNode.__init__(self, 'pybullet_objects_example_node') 16 | self.it = 0 17 | self.add_pybullet_object() 18 | self.Timer(self.Duration(self.interval), self.main_loop) 19 | 20 | 21 | def add_pybullet_object(self): 22 | srv = 'rpbi/add_pybullet_object' 23 | self.wait_for_service(srv) 24 | try: 25 | handle = self.ServiceProxy(srv, AddPybulletObject) 26 | req = PybulletObject() 27 | req.object_type=PybulletObject.DYNAMIC 28 | req.filename='{rpbi_examples}/configs/pybullet_objects_example/dynamic_box.yaml' 29 | resp = handle(req) 30 | if resp.success: 31 | self.loginfo('successfully added object') 32 | else: 33 | self.logwarn('failed to add object: %s' % resp.message) 34 | except Exception as e: 35 | self.logerr('failed to add object: %s' % str(e)) 36 | 37 | 38 | 39 | def remove_object(self): 40 | srv = 'rpbi/remove_pybullet_object' 41 | self.wait_for_service(srv) 42 | try: 43 | handle = self.ServiceProxy(srv, SetString) 44 | req = SetStringRequest(data="dynamic_box") 45 | resp = handle(req) 46 | if resp.success: 47 | self.loginfo('successfully removed object') 48 | else: 49 | self.logwarn('failed to remove object: %s' % resp.message) 50 | except Exception as e: 51 | self.logerr('failed to remove object: %s' % str(e)) 52 | 53 | 54 | def main_loop(self, event): 55 | self.remove_object() 56 | self.add_pybullet_object() 57 | self.it += 1 58 | 59 | 60 | def main(): 61 | Node().spin() 62 | 63 | 64 | if __name__ == '__main__': 65 | main() 66 | -------------------------------------------------------------------------------- /rpbi_examples/configs/run_pose/talos.yaml: -------------------------------------------------------------------------------- 1 | name: "talos" 2 | 3 | loadURDF: 4 | fileName: 'robot_description' 5 | useFixedBase: 1 # use 1 (fixed base) only for demo 6 | basePosition: [0, 0, 1.1] 7 | 8 | is_visual_robot: false 9 | 10 | # initial_joint_position: 11 | # torso_1_joint: 30 12 | # torso_2_joint 13 | # imu_joint 14 | # head_1_joint 15 | # head_2_joint 16 | # rgbd_joint 17 | # rgbd_optical_joint 18 | # rgbd_depth_joint 19 | # rgbd_depth_optical_joint 20 | # rgbd_rgb_joint 21 | # rgbd_rgb_optical_joint 22 | # arm_left_1_joint 23 | # arm_left_2_joint 24 | # arm_left_3_joint 25 | # arm_left_4_joint 26 | # arm_left_5_joint 27 | # arm_left_6_joint 28 | # arm_left_7_joint 29 | # wrist_left_ft_joint 30 | # wrist_left_tool_joint 31 | # gripper_left_base_link_joint 32 | # gripper_left_joint 33 | # gripper_left_inner_double_joint 34 | # gripper_left_fingertip_1_joint 35 | # gripper_left_fingertip_2_joint 36 | # gripper_left_motor_single_joint 37 | # gripper_left_inner_single_joint 38 | # gripper_left_fingertip_3_joint 39 | # arm_right_1_joint 40 | # arm_right_2_joint 41 | # arm_right_3_joint 42 | # arm_right_4_joint 43 | # arm_right_5_joint 44 | # arm_right_6_joint 45 | # arm_right_7_joint 46 | # wrist_right_ft_joint 47 | # wrist_right_tool_joint 48 | # gripper_right_base_link_joint 49 | # gripper_right_joint 50 | # gripper_right_inner_double_joint 51 | # gripper_right_fingertip_1_joint 52 | # gripper_right_fingertip_2_joint 53 | # gripper_right_motor_single_joint 54 | # gripper_right_inner_single_joint 55 | # gripper_right_fingertip_3_joint 56 | # leg_left_1_joint 57 | # leg_left_2_joint 58 | # leg_left_3_joint 59 | # leg_left_4_joint 60 | # leg_left_5_joint 61 | # leg_left_6_joint 62 | # leg_left_sole_fix_joint 63 | # leg_right_1_joint 64 | # leg_right_2_joint 65 | # leg_right_3_joint 66 | # leg_right_4_joint 67 | # leg_right_5_joint 68 | # leg_right_6_joint 69 | # leg_right_sole_fix_joint 70 | 71 | initial_revolute_joint_positions_are_deg: true 72 | 73 | setJointMotorControlArray: 74 | controlMode: "POSITION_CONTROL" 75 | 76 | publish_joint_state_frequency: 30 77 | 78 | broadcast_link_states: true 79 | brodcast_link_states_frequency: 30 80 | -------------------------------------------------------------------------------- /doc/conf.py: -------------------------------------------------------------------------------- 1 | # Configuration file for the Sphinx documentation builder. 2 | # 3 | # This file only contains a selection of the most common options. For a full 4 | # list see the documentation: 5 | # https://www.sphinx-doc.org/en/master/usage/configuration.html 6 | 7 | # -- Path setup -------------------------------------------------------------- 8 | 9 | # If extensions (or modules to document with autodoc) are in another directory, 10 | # add these directories to sys.path here. If the directory is relative to the 11 | # documentation root, use os.path.abspath to make it absolute, like shown here. 12 | # 13 | # import os 14 | # import sys 15 | # sys.path.insert(0, os.path.abspath('.')) 16 | 17 | 18 | # -- Project information ----------------------------------------------------- 19 | 20 | project = 'ROS-PyBullet Interface' 21 | copyright = '2022, Christopher E. Mower, Theodoros Stouraitis, Sethu Vijayakumar, University of Edinburgh' 22 | author = 'Christopher E. Mower' 23 | 24 | 25 | # -- General configuration --------------------------------------------------- 26 | 27 | # Add any Sphinx extension module names here, as strings. They can be 28 | # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom 29 | # ones. 30 | extensions = [ 31 | ] 32 | 33 | # Add any paths that contain templates here, relative to this directory. 34 | templates_path = ['_templates'] 35 | 36 | # List of patterns, relative to source directory, that match files and 37 | # directories to ignore when looking for source files. 38 | # This pattern also affects html_static_path and html_extra_path. 39 | exclude_patterns = ['_build', 'Thumbs.db', '.DS_Store'] 40 | 41 | 42 | # -- Options for HTML output ------------------------------------------------- 43 | 44 | # The theme to use for HTML and HTML Help pages. See the documentation for 45 | # a list of builtin themes. 46 | # 47 | # html_theme = 'alabaster' 48 | html_theme = 'sphinx_rtd_theme' 49 | html_theme_options = { 50 | } 51 | html_show_sourcelink = False 52 | 53 | # Add any paths that contain custom static files (such as style sheets) here, 54 | # relative to this directory. They are copied after the builtin static files, 55 | # so a file named "default.css" will overwrite the builtin "default.css". 56 | html_static_path = ['_static'] 57 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_dynamic_object.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from .pybullet_object import PybulletObject 3 | 4 | 5 | class PybulletDynamicObject(PybulletObject): 6 | 7 | """Objects motion defined by Pybullet.""" 8 | 9 | def init(self): 10 | 11 | # Get visual and collision shape indices 12 | self.base_visual_shape_index = self.create_visual_shape(self.createVisualShape) 13 | self.base_collision_shape_index = self.create_collision_shape(self.createCollisionShape) 14 | 15 | # Setup multi body 16 | self.body_unique_id = self.pb.createMultiBody( 17 | baseMass=self.baseMass, 18 | baseVisualShapeIndex=self.base_visual_shape_index, 19 | baseCollisionShapeIndex=self.base_collision_shape_index, 20 | basePosition=self.basePosition, 21 | baseOrientation=self.baseOrientation, 22 | ) 23 | 24 | # Reset initial velocity 25 | if self.reset_base_velocity is not None: 26 | self.pb.resetBaseVelocity(self.body_unique_id, **self.reset_base_velocity) 27 | 28 | # Set dynamics 29 | self.change_dynamics(self.changeDynamics) 30 | 31 | # Broadcast pose 32 | if self.broadcast_hz > 0: 33 | dt = self.node.Duration(1.0/float(self.broadcast_hz)) 34 | self.timers['broadcast_tf'] = self.node.Timer(dt, self.broadcast) 35 | 36 | def broadcast(self, event): 37 | pos, ori = self.pb.getBasePositionAndOrientation(self.body_unique_id) 38 | self.node.tf.set_tf('rpbi/world', f'rpbi/{self.name}', pos, ori) 39 | 40 | @property 41 | def baseMass(self): 42 | return self.config['baseMass'] 43 | 44 | @property 45 | def basePosition(self): 46 | return self.config.get('basePosition', [0., 0., 0.]) 47 | 48 | @property 49 | def baseOrientation(self): 50 | return self.config.get('baseOrientation', [0., 0., 0., 1.]) 51 | 52 | @property 53 | def createVisualShape(self): 54 | return self.config['createVisualShape'] 55 | 56 | @property 57 | def createCollisionShape(self): 58 | return self.config['createCollisionShape'] 59 | 60 | @property 61 | def changeDynamics(self): 62 | return self.config['changeDynamics'] 63 | 64 | @property 65 | def reset_base_velocity(self): 66 | return self.config.get('resetBaseVelocity') 67 | 68 | @property 69 | def broadcast_hz(self): 70 | return self.config.get('broadcast_hz', 0) 71 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_object_pose.py: -------------------------------------------------------------------------------- 1 | import tf_conversions 2 | import numpy as np 3 | 4 | class PybulletObjectPose: 5 | 6 | def __init__(self, pb_obj): 7 | 8 | # Initial setup 9 | self.pb_obj = pb_obj 10 | self.config = pb_obj.config.get('object_tf', {}) 11 | self.num_debug_thrown = 0 12 | 13 | # Setup pose 14 | self.pose = np.zeros(3), np.array([0., 0., 0., 1.]) 15 | if self.tf_specified(): 16 | # user specified /tf -> start listener 17 | self.pb_obj.timers['pose_listener'] = self.pb_obj.node.Timer(self.dt, self.listener) 18 | # else: pose is always identity 19 | 20 | @property 21 | def tf_id(self): 22 | return self.config.get('tf_id') 23 | 24 | @property 25 | def hz(self): 26 | return int(self.config.get('hz', 30)) 27 | 28 | @property 29 | def dt(self): 30 | return self.pb_obj.node.Duration(1.0/float(self.hz)) 31 | 32 | @property 33 | def max_debug_limit(self): 34 | return self.hz*2 # i.e. 2 secs worth 35 | 36 | def tf_specified(self): 37 | return self.tf_id is not None 38 | 39 | def listener(self, event): 40 | tf = self.pb_obj.node.tf.get_tf_msg('rpbi/world', self.tf_id) 41 | if tf: 42 | self.pose = self.pb_obj.node.tf.msg_to_pos_quat(tf) 43 | 44 | def get(self): 45 | return self.pose # pose is the tuple pos,quat 46 | 47 | def start_reset_pose(self): 48 | self.pb_obj.timers['pose_reset'] = self.pb_obj.node.Timer(self.dt, self.reset_pose) 49 | 50 | def reset_pose(self, event): 51 | 52 | # Check that pybullet object has body unique id 53 | if self.pb_obj.body_unique_id is None: 54 | if self.num_debug_thrown < self.max_debug_limit: 55 | self.pb_obj.node.logdebug(f'body unique id for pybullet object {self.pb_obj.name} is None') 56 | self.num_debug_thrown += 1 57 | return 58 | else: 59 | msg = f'body unique id for pybullet object {self.pb_obj.name} is None max number of iterations without this changing has been reached!' 60 | self.pb_obj.node.logerr(msg) 61 | raise RuntimeError(msg) 62 | 63 | # Reset num_debug_thrown to 0 64 | self.num_debug_thrown = 0 65 | 66 | # Update pose 67 | pos, ori = self.get() 68 | self.pb_obj.pb.resetBasePositionAndOrientation(self.pb_obj.body_unique_id, pos, ori) 69 | -------------------------------------------------------------------------------- /doc/acknowledgements.rst: -------------------------------------------------------------------------------- 1 | Acknowledgements 2 | ================ 3 | 4 | This research is supported by `The Alan Turing Institute `_, United Kingdom and has received funding from the European Union’s Horizon 2020 research and innovation 5 | programme under grant agreement No 101017008, `Enhancing Healthcare with Assistive Robotic Mobile Manipulation (HARMONY) `_. 6 | This work was supported by core funding from the Wellcome/EPSRC [WT203148/Z/16/Z; NS/A000049/1]. 7 | This project has received funding from the European Union’s Horizon 2020 research and innovation programme under grant agreement No 101016985, `Functionally Accurate RObotic Surgery (FAROS project) `_. 8 | This research is supported by `Kawada Robotics Corporation `_ and the `Honda Research Institute Europe `_. 9 | 10 | .. image:: images/UoE.jpg 11 | :width: 300 12 | :alt: The University of Edinburgh Logo 13 | 14 | .. image:: images/KCL-Logo.png 15 | :width: 300 16 | :alt: King's College London Logo 17 | 18 | .. image:: images/ATI_logo.png 19 | :width: 300 20 | :alt: The Alan Turing Institute Logo 21 | 22 | .. image:: images/Harmony_logo.png 23 | :width: 400 24 | :alt: HARMONY Logo 25 | 26 | .. image:: images/FAROS_Logo.png 27 | :width: 150 28 | :alt: FAROS Logo 29 | 30 | The main contributors to the development of the ROS-PyBullet Interface are as follows. 31 | 32 | * `Christopher E. Mower `_, King's College London, UK. 33 | * `Theodoros Stouraitis `_, Honda Research Institute, Offenbach, Germany. 34 | * `Lei Yan `_, Harbin Institute of Technology, Shenzhen, China. 35 | * `João Moura `_, University of Edinburgh, Edinburgh, UK. 36 | * `Christian Rauch `_, University of Edinburgh, Edinburgh, UK. 37 | * `Nazanin Zamani Behabadi `_, London, UK. 38 | * `Michael Gienger `_, Honda Research Institute, Offenbach, Germany. 39 | * `Tom Vercauteren `_, King's College London, UK. 40 | * `Christos Bergeles `_, King's College London, UK. 41 | * `Sethu Vijayakumar `_, University of Edinburgh, Edinburgh, and The Alan Turing Institute, UK. 42 | -------------------------------------------------------------------------------- /install.sh: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # 4 | # This is a script for installing the ROS-PyBullet Interface. Please 5 | # inspect this script before running. Run in a terminal using 6 | # 7 | # $ bash install.sh 8 | # 9 | 10 | echo "Installing the ROS-PyBullet Interface ..." 11 | 12 | # ========================================================================== 13 | # Clone required repositories 14 | cd $(catkin locate)/src 15 | 16 | git clone https://github.com/ros-pybullet/custom_ros_tools.git 17 | git clone https://github.com/ros-pybullet/dmp.git 18 | git clone https://github.com/ros-pybullet/ik_ros.git 19 | git clone https://github.com/ros-pybullet/operator_node.git 20 | git clone https://github.com/ros-pybullet/teleop.git 21 | git clone https://github.com/ros-pybullet/ros-keyboard.git 22 | 23 | 24 | git clone https://github.com/ros-pybullet/ros_kortex.git 25 | mv -v ros_kortex/kortex_description . 26 | rm -rfv ros_kortex 27 | 28 | git clone https://github.com/ros-pybullet/nextagea.git 29 | mv -v nextagea/nextagea_description . 30 | rm -rfv nextagea 31 | 32 | git clone -b UoE_SLMC https://github.com/ros-pybullet/talos_robot.git 33 | git clone https://github.com/ros-pybullet/exotica.git 34 | 35 | git clone https://github.com/ros-pybullet/trac_ik.git 36 | 37 | # ========================================================================== 38 | # Install dependancies 39 | 40 | # Install SDL (required by ros-keyboard) 41 | # https://www.libsdl.org 42 | sudo apt install libsdl2-dev libsdl2-2.0-0 43 | 44 | # Update rosdep and install deps 45 | rosdep update 46 | rosdep install --from-paths ./ -iry 47 | 48 | # ========================================================================== 49 | # Build/source workspace 50 | catkin build -s 51 | 52 | echo "Completed installation of the ROS-PyBullet Interface" 53 | echo " " 54 | echo " >>>> please run $ source $(catkin locate)/devel/setup.bash" 55 | echo " " 56 | echo "Please try out any of the following examples" 57 | echo " " 58 | echo "roslaunch rpbi_examples basic_example_kuka_lwr.launch" 59 | echo "roslaunch rpbi_examples basic_example_human_model.launch" 60 | echo "roslaunch rpbi_examples basic_example_talos.launch" 61 | echo "roslaunch rpbi_examples pybullet_objects_example.launch" 62 | echo "roslaunch rpbi_examples soft_body.launch" 63 | echo "roslaunch rpbi_examples basic_example_kinova.launch" 64 | echo "roslaunch rpbi_examples basic_example_nextage.launch" 65 | echo "roslaunch rpbi_examples lfd.launch" 66 | echo "roslaunch rpbi_examples pybullet_rgbd_example.launch" 67 | echo "roslaunch rpbi_examples interpolation.launch" 68 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_soft_body.py: -------------------------------------------------------------------------------- 1 | from .pybullet_object import PybulletObject 2 | from custom_ros_tools.config import replace_package 3 | 4 | class PybulletSoftBodyObject(PybulletObject): 5 | 6 | def init(self): 7 | 8 | if self.load_method() == 'loadSoftBody': 9 | self.body_unique_id = self.pb.loadSoftBody(**self.load_soft_body) 10 | elif sself.load_method() == 'loadURDF': 11 | self.body_unique_id = self.pb.loadURDF(**self.load_urdf) 12 | 13 | for anchor in self.create_soft_body_anchor: 14 | self.pb.createSoftBodyAnchor(*anchor) 15 | 16 | def load_method(self): 17 | if 'loadSoftBody' in self.config: 18 | return 'loadSoftBody' 19 | elif 'loadURDF' in self.config: 20 | return 'loadURDF' 21 | 22 | @property 23 | def create_soft_body_anchor(self): 24 | """ 25 | 26 | NOTE: the documentation for createSoftBodyAnchor is 27 | limited. It is not clear what is exactly the interface. 28 | 29 | In the config, you can create anchors by parsing a list to 30 | createSoftBodyAnchor, e.g. 31 | 32 | createSoftBodyAnchor: 33 | - [A1, A2, ...] 34 | - [A1, A2, ...] 35 | 36 | The soft body unique ID will be passed automatically, but any 37 | other parameters must be supplied. Some potential resources: 38 | 39 | - https://github.com/bulletphysics/bullet3/blob/master/examples/pybullet/examples/deformable_anchor.py 40 | - https://github.com/bulletphysics/bullet3/discussions/4088 41 | - https://github.com/bulletphysics/bullet3/blob/7dee3436e747958e7088dfdcea0e4ae031ce619e/examples/pybullet/pybullet.c#L2280-L2326 42 | 43 | """ 44 | return self.config.get('createSoftBodyAnchor', []) 45 | 46 | @property 47 | def load_urdf(self): 48 | load_urdf = self.config['loadURDF'] 49 | load_urdf['fileName'] = replace_package(load_urdf['fileName']) 50 | if 'flags' in load_urdf: 51 | load_urdf['flags'] = self.node.parse_options(load_urdf['flags']) 52 | return load_urdf 53 | 54 | 55 | @property 56 | def load_soft_body(self): 57 | load_soft_body = self.config['loadSoftBody'] 58 | if 'fileName' in load_soft_body: 59 | load_soft_body['fileName'] = replace_package(load_soft_body['fileName']) 60 | if 'simFileName' in load_soft_body: 61 | load_soft_body['simFileName'] = replace_package(load_soft_body['simFileName']) 62 | return load_soft_body 63 | 64 | -------------------------------------------------------------------------------- /rpbi_examples/configs/rgbd.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 0 4 | Name: Displays 5 | Preferences: 6 | PromptSaveOnExit: false 7 | # Toolbars: 8 | # toolButtonStyle: 2 9 | Visualization Manager: 10 | Class: "" 11 | Displays: 12 | - Class: rviz/Grid 13 | Enabled: true 14 | Line Style: 15 | Line Width: 0.03 16 | Value: Lines 17 | - Class: rviz/TF 18 | Enabled: true 19 | Frame Timeout: 3600 20 | Marker Alpha: 1 21 | Marker Scale: 1 22 | Name: TF 23 | Show Arrows: false 24 | Show Axes: true 25 | Show Names: false 26 | Update Interval: 0 27 | Value: true 28 | - Alpha: 1 29 | Auto Size: 30 | Auto Size Factor: 1 31 | Value: true 32 | Autocompute Intensity Bounds: true 33 | Autocompute Value Bounds: 34 | Max Value: 10 35 | Min Value: -10 36 | Value: true 37 | Axis: Z 38 | Channel Name: intensity 39 | Class: rviz/DepthCloud 40 | Color: 255; 255; 255 41 | Color Image Topic: /rpbi/camera/colour/image 42 | Color Transformer: RGB8 43 | Color Transport Hint: raw 44 | Decay Time: 0 45 | Depth Map Topic: /rpbi/camera/depth/image 46 | Depth Map Transport Hint: raw 47 | Enabled: true 48 | Invert Rainbow: false 49 | Max Color: 255; 255; 255 50 | Min Color: 0; 0; 0 51 | Name: DepthCloud 52 | Occlusion Compensation: 53 | Occlusion Time-Out: 30 54 | Value: false 55 | Position Transformer: XYZ 56 | Queue Size: 5 57 | Selectable: true 58 | Size (Pixels): 3 59 | Style: Points 60 | Topic Filter: true 61 | Use Fixed Frame: true 62 | Use rainbow: true 63 | Value: true 64 | Enabled: true 65 | Global Options: 66 | Background Color: 48; 48; 48 67 | Default Light: true 68 | Fixed Frame: rpbi/world 69 | Frame Rate: 30 70 | Name: root 71 | Tools: 72 | - Class: rviz/Interact 73 | Hide Inactive Objects: true 74 | - Class: rviz/MoveCamera 75 | - Class: rviz/Select 76 | - Class: rviz/FocusCamera 77 | - Class: rviz/Measure 78 | Value: true 79 | Views: 80 | Current: 81 | Class: rviz/Orbit 82 | Distance: 5 83 | Enable Stereo Rendering: 84 | Stereo Eye Separation: 0.06 85 | Stereo Focal Distance: 1 86 | Swap Stereo Eyes: false 87 | Value: false 88 | Field of View: 0.78 89 | Focal Shape Fixed Size: false 90 | Focal Shape Size: 0.05000000074505806 91 | Invert Z Axis: false 92 | Name: Current View 93 | Near Clip Distance: 0.01 94 | Pitch: 0.85 95 | Target Frame: 96 | Yaw: 4.71 97 | Saved: ~ 98 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_robot_ik.py: -------------------------------------------------------------------------------- 1 | from sensor_msgs.msg import JointState 2 | from ros_pybullet_interface.msg import CalculateInverseKinematicsProblem 3 | 4 | class Ik: 5 | 6 | def __init__(self, pb_obj, joints): 7 | self.pb_obj = pb_obj 8 | self.joints = joints 9 | 10 | if self.start_ik_callback: 11 | topic = f'rpbi/{self.pb_obj.name}/ik' 12 | self.pb_obj.node.Subscriber(topic, CalculateInverseKinematicsProblem, self.callback) 13 | 14 | @property 15 | def start_ik_callback(self): 16 | return self.pb_obj.config.get('start_ik_callback', False) 17 | 18 | def callback(self, problem): 19 | self.joints.joint_state_target_callback(self.solve(problem)) 20 | 21 | def solve(self, problem): 22 | 23 | # Setup arguments for pybullet.calculateInverseKinematics 24 | args = [ 25 | self.pb_obj.body_unique_id, 26 | self.joints.link_names.index(problem.link_name), 27 | problem.targetPosition[:3] 28 | ] 29 | 30 | # Setup keyword arguments for pybullet.calculateInverseKinematics 31 | kwargs = {} 32 | if problem.targetOrientation: 33 | kwargs['targetOrientation'] = problem.targetOrientation 34 | if problem.lowerLimits: 35 | kwargs['lowerLimits'] = problem.lowerLimits 36 | if problem.upperLimits: 37 | kwargs['upperLimits'] = problem.upperLimits 38 | if problem.jointRanges: 39 | kwargs['jointRanges'] = problem.jointRanges 40 | if problem.resetPoses: 41 | kwargs['resetPoses'] = problem.resetPoses 42 | if problem.jointDamping: 43 | kwargs['jointDamping'] = problem.jointDamping 44 | if problem.solver: 45 | kwargs['solver'] = problem.solver 46 | # if problem.currentPosition: 47 | # kwargs['currentPosition'] = problem.currentPosition 48 | if problem.maxNumIterations > 0: 49 | kwargs['maxNumIterations'] = problem.maxNumIterations 50 | if problem.residualThreshold: 51 | kwargs['residualThreshold'] = problem.residualThreshold 52 | 53 | # Solve the problem 54 | positions = self.pb_obj.pb.calculateInverseKinematics(*args, **kwargs) 55 | 56 | # Pack solution 57 | names = [ 58 | joint.jointName for joint in self.joints 59 | if not joint.is_fixed() 60 | ] 61 | solution = JointState(name=names, position=positions) 62 | solution.header.stamp = self.pb_obj.node.time_now() 63 | 64 | if (problem.dt>0.0) and problem.currentPosition: 65 | dt = problem.dt 66 | solution.velocity = [(p-c)/dt for p, c in zip(positions, problem.currentPosition)] 67 | 68 | return solution 69 | -------------------------------------------------------------------------------- /rpbi_examples/scripts/orbit_node.py: -------------------------------------------------------------------------------- 1 | import rospy 2 | import numpy as np 3 | from custom_ros_tools.tf import TfInterface 4 | from geometry_msgs.msg import TransformStamped, Transform 5 | from custom_ros_tools.ros_comm import ToggleService 6 | from tf_conversions import transformations 7 | 8 | 9 | def look_at(frame_from, frame_to): 10 | normalise = lambda x : x / np.linalg.norm(x) 11 | 12 | forward = normalise(frame_to - frame_from) 13 | right = normalise(np.cross(forward, normalise([0, 0, 1]))) 14 | down = np.cross(forward, right) 15 | 16 | T = np.identity(4) 17 | T[:3, 0] = right 18 | T[:3, 1] = down 19 | T[:3, 2] = forward 20 | T[:3, 3] = frame_from 21 | 22 | return T 23 | 24 | 25 | def transform_to_msg(transformation): 26 | pos = transformations.translation_from_matrix(transformation) # x, y, z 27 | quat = transformations.quaternion_from_matrix(transformation) # x, y, z, w 28 | 29 | t = Transform() 30 | t.translation.x, t.translation.y, t.translation.z = pos 31 | t.rotation.x, t.rotation.y, t.rotation.z, t.rotation.w = quat 32 | 33 | return t 34 | 35 | 36 | class OrbitNode: 37 | def __init__(self, base_frame, target_frame, centre, distance, height): 38 | 39 | # Setup ros node 40 | rospy.init_node('orbit_node') 41 | 42 | # Get parameters 43 | self.duration = rospy.Duration(1.0/rospy.get_param('~hz', 50)) 44 | tf_ns = rospy.get_param('~tf_ns', '') 45 | 46 | self.target = centre 47 | # self.distance = distance 48 | self.distance_xy = np.sqrt(distance**2 - height**2) 49 | self.height = height 50 | self.speed = 1/8 * np.pi # rad/s 51 | 52 | # Setup transform and broadcaster 53 | self.tf_interface = TfInterface() 54 | self.tf = TransformStamped() 55 | self.tf.header.frame_id = base_frame 56 | self.tf.child_frame_id = target_frame 57 | if tf_ns: 58 | self.tf.header.frame_id = tf_ns + '/' + self.tf.header.frame_id 59 | self.tf.child_frame_id = tf_ns + '/' + self.tf.child_frame_id 60 | 61 | self.start_time = rospy.Time.now() 62 | self.timer = rospy.Timer(self.duration, self.main_loop) 63 | 64 | def main_loop(self, event): 65 | self.tf.header.stamp = rospy.Time.now() 66 | 67 | t = (self.tf.header.stamp - self.start_time).to_sec() 68 | 69 | # source position at "distance" from target point 70 | a = self.speed * t 71 | x = np.cos(a) * self.distance_xy 72 | y = np.sin(a) * self.distance_xy 73 | source = np.array([x, y, self.height]) 74 | 75 | T = look_at(source, self.target) 76 | self.tf.transform = transform_to_msg(T) 77 | 78 | self.tf_interface.tf_broadcaster.sendTransform(self.tf) 79 | 80 | def spin(self): 81 | rospy.spin() 82 | 83 | 84 | if __name__ == '__main__': 85 | OrbitNode("rpbi/world", "rpbi/camera", [0,0,0], 2, 1.5).spin() 86 | -------------------------------------------------------------------------------- /rpbi_examples/launch/interpolation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 39 | 40 | 41 | 43 | 44 | 45 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /rpbi_examples/launch/teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /rpbi_examples/scripts/run_human_interaction.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import sys 3 | import copy 4 | import rospy 5 | import numpy as np 6 | from std_msgs.msg import Float64 7 | from custom_ros_tools.tf import TfInterface 8 | from geometry_msgs.msg import PoseStamped, Vector3, WrenchStamped 9 | from geomagic_touch_x_ros.msg import OmniFeedback 10 | 11 | FF_SCALE = 1 12 | 13 | class Node: 14 | 15 | def __init__(self): 16 | rospy.init_node('run_human_interaction_node') 17 | touchx_name = rospy.get_param('~device_name', 'Left') 18 | self.tf = TfInterface() 19 | self.dev_pos = None 20 | self.ft_reading_corrected = None 21 | self.zpos = None 22 | self.ft_reading = None 23 | self.zforce_offset = 0.0 24 | rospy.Subscriber(f'{touchx_name}/Pose', PoseStamped, self.pose_callback) 25 | rospy.Subscriber('rpbi/kuka_lwr/force_torque_sensor_mount/ft_sensor', WrenchStamped, self.force_feedback_callback) 26 | self.ff_pub = rospy.Publisher(f'{touchx_name}/force_feedback', OmniFeedback, queue_size=10) 27 | self.ft_reading_corrected_pub = rospy.Publisher('ft_reading', Float64, queue_size=10) 28 | self.calibrate_ft_sensor() 29 | rospy.sleep(5.0) 30 | rospy.loginfo('demo ready') 31 | rospy.Timer(rospy.Duration(1.0/float(rospy.get_param('~hz', 400))), self.main_loop) 32 | 33 | def calibrate_ft_sensor(self): 34 | rospy.sleep(1.0) 35 | 36 | r = rospy.Rate(100) 37 | 38 | maxit = 100 39 | for i in range(maxit): 40 | if self.ft_reading is not None: 41 | break 42 | r.sleep() 43 | else: 44 | rospy.logerr('>>>no ft reading<<<') 45 | sys.exit(0) 46 | 47 | rospy.loginfo('calibrating ft_sensor....') 48 | num = 100 49 | data = np.zeros(num) 50 | 51 | for i in range(num): 52 | data[i] = copy.deepcopy(self.ft_reading) 53 | 54 | self.zforce_offset = data.mean() 55 | 56 | rospy.loginfo('calibrated') 57 | 58 | def pose_callback(self, msg): 59 | self.dev_pos = Vector3( 60 | x=msg.pose.position.x, 61 | y=msg.pose.position.y, 62 | z=msg.pose.position.z, 63 | ) 64 | self.zpos = np.clip(-12*msg.pose.position.z, -10.0, 0.08) 65 | 66 | def force_feedback_callback(self, msg): 67 | self.ft_reading = msg.wrench.force.z 68 | self.ft_reading_corrected = self.ft_reading - self.zforce_offset 69 | self.ft_reading_corrected_pub.publish(Float64(data=self.ft_reading_corrected)) 70 | 71 | def main_loop(self, event): 72 | if self.zpos is None: return 73 | 74 | # Specify ik target 75 | offsetz = -0.1 76 | self.tf.set_tf('teleop_origin', 'teleop_target', [0, 0, self.zpos-offsetz]) 77 | 78 | if self.dev_pos is None: return 79 | if self.ft_reading is None: return 80 | 81 | # Get force feedback 82 | f = -FF_SCALE*self.ft_reading_corrected 83 | ff = Vector3(x=0, y=f, z=0) 84 | 85 | # Apply force feeback 86 | self.ff_pub.publish(OmniFeedback(position=self.dev_pos, force=ff)) 87 | 88 | def spin(self): 89 | rospy.spin() 90 | 91 | def main(): 92 | Node().spin() 93 | 94 | if __name__ == '__main__': 95 | main() 96 | -------------------------------------------------------------------------------- /rpbi_examples/launch/human_interaction.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | -------------------------------------------------------------------------------- /rpbi_utils/scripts/rpbi_controls_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import tkinter 3 | from std_msgs.msg import Int64 4 | from rpbi.ros_node import RosNode 5 | from std_srvs.srv import Trigger, TriggerResponse 6 | from sensor_msgs.msg import JointState 7 | from ros_pybullet_interface.srv import ResetJointState 8 | from custom_ros_tools.ros_comm import get_srv_handler 9 | 10 | class Window(tkinter.Frame): 11 | 12 | def __init__(self, node, master): 13 | tkinter.Frame.__init__(self, master) 14 | self.master = master 15 | self.node = node 16 | 17 | # widget can take all window 18 | self.pack(fill=tkinter.BOTH, expand=1) 19 | 20 | # create button, link it to clickExitButton() 21 | self.start_button = tkinter.Button(self, text="Start", command=self.start_button_handler) 22 | self.step_button = tkinter.Button(self, text="Step", command=self.step_button_handler) 23 | self.stop_button = tkinter.Button(self, text="Stop", command=self.stop_button_handler) 24 | self.send_button = tkinter.Button(self, text='Send', command=self.send_button_handler) 25 | 26 | self.start_button.place(x=0, y=0) 27 | self.step_button.place(x=0, y=100) 28 | self.stop_button.place(x=0, y=200) 29 | self.send_button.place(x=100, y=0) 30 | 31 | self.text_box = tkinter.Text(self, height=100, width=200) 32 | self.text_box.place(x=100, y=300) 33 | self.text_box.insert(tkinter.END, "Initialization") 34 | 35 | self.master.wm_title("ROS-PyBullet Interface Controls") 36 | self.master.geometry("500x500") 37 | 38 | def start_button_handler(self): 39 | self.node.start_button_handler() 40 | 41 | def step_button_handler(self): 42 | self.node.step_button_handler() 43 | 44 | def stop_button_handler(self): 45 | self.node.stop_button_handler() 46 | 47 | def send_button_handler(self): 48 | self.node.send_robot() 49 | 50 | class Node(RosNode): 51 | 52 | def __init__(self): 53 | super().__init__('rpbi_controls_node') 54 | self.on_shutdown(self.close) 55 | self.config = self.get_param('~config', {}).get('controls', {}) 56 | self.Subscriber('rpbi/status', Int64, self.status_callback) 57 | self.start_button_handler = get_srv_handler('rpbi/start', Trigger, persistent=True) 58 | self.step_button_handler = get_srv_handler('rpbi/step', Trigger, persistent=True) 59 | self.stop_button_handler = get_srv_handler('rpbi/stop', Trigger, persistent=True) 60 | 61 | robot_name = self.config.get('robot_name', None) 62 | if robot_name: 63 | self.send_button_handler = get_srv_handler(f'rpbi/{robot_name}/move_to_initial_joint_state', ResetJointState, persistent=True) 64 | else: 65 | self.send_button_handler = None 66 | self.root = tkinter.Tk() 67 | self.app = Window(self, self.root) 68 | 69 | def status_callback(self, msg): 70 | if not hasattr(self, 'app'): return 71 | self.app.text_box.delete("1.0","end") 72 | self.app.text_box.insert(tkinter.END, f"Status: {msg.data}") 73 | 74 | def send_robot(self, state=None): 75 | if self.send_button_handler: 76 | self.send_button_handler(JointState(), 6.0) 77 | 78 | def close(self): 79 | self.root.quit() 80 | 81 | def spin(self): 82 | self.root.mainloop() 83 | 84 | def main(): 85 | Node().spin() 86 | 87 | if __name__ == '__main__': 88 | main() 89 | -------------------------------------------------------------------------------- /rpbi_examples/launch/lfd.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | -------------------------------------------------------------------------------- /rpbi_examples/scripts/basic_robot_example_node.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import sys 3 | import math 4 | import rospy 5 | from std_msgs.msg import Int64 6 | from sensor_msgs.msg import JointState 7 | from ros_pybullet_interface.srv import RobotInfo 8 | from ros_pybullet_interface.srv import ResetJointState, ResetJointStateRequest 9 | from custom_ros_tools.ros_comm import get_srv_handler 10 | 11 | class Node: 12 | 13 | hz = 50 14 | dt = 1.0/float(hz) 15 | 16 | def __init__(self): 17 | 18 | # Init node 19 | rospy.init_node('basic_robot_example_node') 20 | 21 | # Get robot name 22 | if rospy.has_param('~robot_name'): 23 | robot_name = rospy.get_param('~robot_name') 24 | elif len(sys.argv) > 1: 25 | robot_name = sys.argv[1] 26 | else: 27 | raise ValueError("robot name not specified, either set the '~robot_name' node parameter or via command line args") 28 | target_joint_state_topic = f'rpbi/{robot_name}/joint_states/target' 29 | 30 | # Other setup 31 | dur = rospy.Duration(self.dt) 32 | 33 | # Subscribers 34 | self.active = False 35 | rospy.Subscriber('rpbi/status', Int64, self.active_callback) 36 | 37 | # Get ndof 38 | handle = get_srv_handler(f'rpbi/{robot_name}/robot_info', RobotInfo) 39 | res = handle() 40 | self.ndof = res.numDof 41 | self.name = [j.jointName for j in res.joint_info if j.jointTypeStr != 'JOINT_FIXED'] 42 | 43 | # Setup for joint updates 44 | self.joint_index = 0 45 | self.position = [0.0]*self.ndof 46 | self.d = 1 47 | self.traj_index = 0 48 | self.joint_traj = [math.sin(0.5*2.0*math.pi*float(i)/100.0) for i in range(200)] 49 | 50 | # Move robot to initial goal state 51 | rospy.loginfo('moving robot to initial joint state') 52 | duration = 3.0 53 | handle = get_srv_handler(f'rpbi/{robot_name}/move_to_joint_state', ResetJointState) 54 | handle(self.get_goal_joint_state(), duration) 55 | 56 | # Setup joint target state publisher 57 | self.pub = rospy.Publisher(target_joint_state_topic, JointState, queue_size=10) 58 | 59 | # Start timer 60 | rospy.Timer(dur, self.publish_joint_state) 61 | 62 | rospy.loginfo('initialized basic example') 63 | 64 | def active_callback(self, msg): 65 | self.active = bool(msg.data) 66 | 67 | def update_joint_index(self): 68 | self.joint_index += self.d 69 | if self.joint_index == self.ndof: 70 | self.joint_index -= 2 71 | self.d = -1 72 | if self.joint_index == -1: 73 | self.joint_index = 0 74 | self.d = 1 75 | 76 | def update_trajectory_index(self): 77 | self.traj_index += 1 78 | if self.traj_index == len(self.joint_traj): 79 | self.traj_index = 0 80 | self.position = [0.0]*self.ndof 81 | self.update_joint_index() 82 | 83 | def get_goal_joint_state(self): 84 | return JointState(name=self.name, position=self.position) 85 | 86 | def publish_joint_state(self, event): 87 | if not self.active: return 88 | self.position[self.joint_index] = self.joint_traj[self.traj_index] 89 | self.pub.publish(self.get_goal_joint_state()) 90 | self.update_trajectory_index() 91 | 92 | def spin(self): 93 | rospy.spin() 94 | 95 | 96 | def main(): 97 | Node().spin() 98 | 99 | 100 | if __name__=='__main__': 101 | main() 102 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_object.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from custom_ros_tools.config import replace_package 3 | from abc import ABC, abstractmethod 4 | 5 | 6 | class PybulletObject(ABC): 7 | 8 | 9 | """Base class for objects in Pybullet.""" 10 | 11 | 12 | def __init__(self, pb, node, config): 13 | 14 | # Set pybullet instance and ROS node 15 | self.pb = pb 16 | self.node = node 17 | self.pubs = {} 18 | self.srvs = {} 19 | self.subs = {} 20 | self.timers = {} 21 | 22 | # Init config 23 | self.config = config 24 | self.name = self.config['name'] 25 | del self.config['name'] 26 | 27 | # Setup variables 28 | self.body_unique_id = None 29 | self.offset = None # object offset in object base frame 30 | self.base = None # object base frame defined in rpbi/world 31 | self.visual_shape_type = None # set when create_visual_shape is called 32 | 33 | # Initialize object 34 | self.init() 35 | 36 | self.node.loginfo(f'added pybullet object {self.name}') 37 | 38 | 39 | @abstractmethod 40 | def init(self): 41 | pass 42 | 43 | 44 | def create_visual_shape(self, config): 45 | """Exposes createVisualShape.""" 46 | if isinstance(config['shapeType'], str): 47 | config['shapeType'] = getattr(self.pb, config['shapeType']) 48 | self.visual_shape_type = config['shapeType'] 49 | if 'fileName' in config.keys(): 50 | config['fileName'] = replace_package(config['fileName']) 51 | return self.pb.createVisualShape(**config) 52 | 53 | 54 | def create_collision_shape(self, config): 55 | """Exposes createCollisionShape.""" 56 | if isinstance(config['shapeType'], str): 57 | config['shapeType'] = getattr(self.pb, config['shapeType']) 58 | if 'fileName' in config.keys(): 59 | config['fileName'] = replace_package(config['fileName']) 60 | return self.pb.createCollisionShape(**config) 61 | 62 | 63 | def change_dynamics(self, config, link_index=-1): 64 | """Exposes changeDynamics.""" 65 | config['bodyUniqueId'] = self.body_unique_id 66 | config['linkIndex'] = link_index 67 | if 'activationState' in config.keys(): 68 | config['activationState'] = getattr(self.pb, config['activationState']) 69 | self.pb.changeDynamics(**config) 70 | 71 | 72 | def destroy(self): 73 | """Removes the object from Pybullet and closes any communication with ROS.""" 74 | 75 | # Close all ROS communication 76 | for t in self.timers.values(): 77 | t.shutdown() 78 | for s in self.subs.values(): 79 | s.unregister() 80 | for p in self.pubs.values(): 81 | p.unregister() 82 | for s in self.srvs.values(): 83 | s.shutdown() 84 | 85 | # Remove object from pybullet 86 | self.pb.removeBody(self.body_unique_id) 87 | 88 | 89 | class PybulletObjectArray: 90 | 91 | 92 | def __init__(self, pb, node, config, object_type, num_objects): 93 | 94 | # Set pybullet instance and ROS node 95 | self.pb = pb 96 | self.node = node 97 | 98 | # Create object array 99 | self.objects = [] 100 | for i in range(num_objects): 101 | 102 | # Update config 103 | config_i = config.copy() 104 | config_i['name'] = config['name'] + str(i) 105 | if 'object_base_tf_frame_id' in config.keys(): 106 | if config['object_base_tf_frame_id'] != 'rpbi/world': 107 | config_i['object_base_tf_frame_id'] = config['object_base_tf_frame_id'] + str(i) 108 | if 'tf_frame_id' in config.keys(): 109 | config_i['tf_frame_id'] = config['tf_frame_id'] + str(i) 110 | 111 | # Append object 112 | self.objects.append(object_type(pb, node, config_i)) 113 | 114 | def destroy(self): 115 | for obj in self.objects: 116 | obj.destroy() 117 | -------------------------------------------------------------------------------- /rpbi_examples/scripts/run_teleop.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import time 3 | import rospy 4 | import numpy as np 5 | from custom_ros_tools.tf import TfInterface 6 | from keyboard.msg import Key 7 | from std_srvs.srv import SetBool, Trigger 8 | from custom_ros_tools.ros_comm import get_srv_handler 9 | from ros_pybullet_interface.msg import CalculateInverseKinematicsProblem 10 | from ros_pybullet_interface.srv import ResetEffState, ResetEffStateRequest 11 | 12 | class TfTracker: 13 | 14 | def __init__(self, tf, parent, child): 15 | self.tf = tf 16 | self.parent = parent 17 | self.child = child 18 | rospy.Timer(rospy.Duration(0.01), self.main_loop) 19 | self.transform = None 20 | 21 | def main_loop(self, event): 22 | self.transform = self.tf.get_tf_msg(self.parent, self.child) 23 | 24 | def distance(self): 25 | if self.transform is None: 26 | return np.inf 27 | return np.linalg.norm(self.tf.msg_to_pos(self.transform)) 28 | 29 | 30 | class Node: 31 | 32 | def __init__(self): 33 | rospy.init_node('run_teleop_node') 34 | self.tf = TfInterface() 35 | self.eff = TfTracker(self.tf, 'teleop_origin', 'rpbi/kuka_lwr/end_effector_ball') 36 | self.teleop_is_on = False 37 | 38 | # Get service handlers 39 | self.move_to_eff_state = get_srv_handler(f'rpbi/kuka_lwr/move_to_eff_state', ResetEffState, persistent=True) 40 | self.reset_teleop_transform = get_srv_handler('reset_teleop_transform_to_zero', Trigger, persistent=True) 41 | 42 | self.toggle_human_interface = get_srv_handler('operator_node/toggle', SetBool, persistent=True) 43 | self.toggle_teleop_tf = get_srv_handler('toggle_teleop_tf', SetBool, persistent=True) 44 | self.toggle_ik_setup = get_srv_handler('ik/setup/trac_ik/toggle', SetBool, persistent=True) 45 | self.toggle_ik_solver = get_srv_handler('ik/solver/trac_ik/toggle', SetBool, persistent=True) 46 | 47 | rospy.Subscriber('keyboard/keydown', Key, self.keyboard_callback) 48 | 49 | def keyboard_callback(self, msg): 50 | if msg.code == Key.KEY_1: 51 | self.move_to_initial_pose() 52 | elif msg.code == Key.KEY_2: 53 | if not self.teleop_is_on: 54 | self.start_teleop() 55 | else: 56 | self.stop_teleop() 57 | 58 | def move_to_initial_pose(self): 59 | 60 | if self.teleop_is_on: 61 | rospy.logwarn('teleop is on') 62 | return 63 | 64 | rospy.loginfo('moving robot to start state ...') 65 | 66 | # Get eff target 67 | timeout = 10.0 68 | start_time = time.time() 69 | while (time.time()-start_time) < timeout: 70 | init_eff_pos, init_eff_rot = self.tf.get_tf('rpbi/world', 'teleop_origin') 71 | if init_eff_pos is not None: 72 | break 73 | else: 74 | rospy.logerr("could not recieve end-effector target transform") 75 | sys.exit(0) 76 | 77 | # Setup problem 78 | problem = CalculateInverseKinematicsProblem() 79 | problem.link_name = 'end_effector_ball' 80 | problem.targetPosition = init_eff_pos 81 | problem.targetOrientation = init_eff_rot 82 | 83 | # Setup request 84 | duration = 2.0 85 | req = ResetEffStateRequest(problem=problem, duration=duration) 86 | 87 | # Move robot 88 | self.move_to_eff_state(req) 89 | 90 | rospy.loginfo('moved robot to start state') 91 | 92 | def start_teleop(self): 93 | 94 | if self.teleop_is_on: 95 | rospy.logwarn('teleop is on') 96 | return 97 | 98 | if self.eff.distance() > 0.03: 99 | rospy.logwarn('end effector is too far away from start pose, press 1 to move robot to intial pose') 100 | return 101 | 102 | # Ensure teleop transform is zero 103 | self.reset_teleop_transform() 104 | 105 | rospy.loginfo('started teleoperation') 106 | 107 | self.toggle_human_interface(True) 108 | self.toggle_teleop_tf(True) 109 | self.toggle_ik_setup(True) 110 | self.toggle_ik_solver(True) 111 | 112 | self.teleop_is_on = True 113 | 114 | def stop_teleop(self): 115 | self.toggle_human_interface(False) 116 | self.toggle_teleop_tf(False) 117 | self.toggle_ik_setup(False) 118 | self.toggle_ik_solver(False) 119 | self.teleop_is_on = False 120 | rospy.loginfo('stopped teleoperation') 121 | 122 | def spin(self): 123 | rospy.spin() 124 | 125 | def main(): 126 | Node().spin() 127 | 128 | if __name__ == '__main__': 129 | main() 130 | -------------------------------------------------------------------------------- /doc/communication_with_ros.rst: -------------------------------------------------------------------------------- 1 | Communication with ROS 2 | ====================== 3 | 4 | In this section, we discuss generally the kind of communication that the interface has with ROS. 5 | This is mainly in terms of topics/services. 6 | For PyBullet object specific ROS communication, see previous sections. 7 | 8 | Services 9 | -------- 10 | 11 | Several services are instantiated when the ROS-PyBullet Interface node is launched. 12 | These allow you to either programatically interact with PyBullet from your own code, or from the command line. 13 | These serives are detailed as follows. 14 | 15 | ``rpbi/start`` (``std_srv/Trigger``) 16 | #################################### 17 | 18 | The simulation is started, if it has been stopped. 19 | 20 | 21 | ``rpbi/step`` (``std_srv/Trigger``) 22 | ################################### 23 | 24 | If the simulation is stopped, then it is stepped by one time-step. 25 | The duration of the time-step is given by the ``timeStep`` parameter in the main configuration file. 26 | 27 | 28 | ``rpbi/stop`` (``std_srv/Trigger``) 29 | ################################### 30 | 31 | The simulation is stopped, if it is running. 32 | 33 | ``rpbi/get_debug_visualizer_camera`` (``ros_pybullet_interface/GetDebugVisualizerCamera``) 34 | ########################################################################################## 35 | 36 | When this service is called, the response returns the current parameters for the main visualizer: i.e. 37 | ``cameraDistance``, 38 | ``cameraYaw``, 39 | ``cameraPitch``, and 40 | ``cameraTargetPosition``. 41 | 42 | ``rpbi/add_pybullet_object`` (``ros_pybullet_interface/AddPybulletObject``) 43 | ########################################################################### 44 | 45 | An object is added to PyBullet. 46 | The service allows you to either load from file or pass the configuration for the object. 47 | The input for the service expects a ``ros_pybullet_interface/PybulletObject`` message - see `here `_. 48 | 49 | For both cases (i.e. load from filename or configuration), an ``object_type`` must be given. 50 | Either ``PybulletObject.VISUAL``, ``PybulletObject.COLLISION``, ``PybulletObject.DYNAMIC``, ``PybulletObject.ROBOT``, ``PybulletObject.SOFT``, or ``PybulletObject.URDF``. 51 | 52 | *Load from filename*: the ``PybulletObject.filename`` variable must be set. 53 | You can specify reletive filenames by giving a ROS package name in the format ``{ros_package}/path/to/file.yaml``. 54 | 55 | *Load from configuration*: if you want to pass the configuration in the service then you need to send it as a string, that is ultimately a yaml file. In Python, the best way to do this is to specify the configration in a ``dict`` (in the same way a yaml file is loaded) and convert it to a string using the ``config_to_str`` method provided in ``custom_ros_tools.config``. 56 | See example below. 57 | 58 | .. code-block:: python 59 | 60 | from custom_ros_tools.config import config_to_str 61 | from ros_pybullet_interface.msg import PybulletObject 62 | 63 | # make config 64 | config = {} 65 | # ... 66 | 67 | # Setup request 68 | req = PybulletObject(config=config_to_str(config)) 69 | 70 | **Note**: loading from a filename takes precedence - if you want to load by passing the configuration then the ``filename`` parameter must not be set. 71 | 72 | ``rpbi/remove_pybullet_object`` (``cob_srv/SetString``) 73 | ####################################################### 74 | 75 | Given the PyBullet object name as the only parameter, the object is removed from PyBullet, and all ROS communication for that object is closed. 76 | 77 | PyBullet Status 78 | --------------- 79 | 80 | From when the ROS-PyBullet node is launched, the state of PyBullet is published to the topic ``rpbi/status`` (with type ``std_msgs/Int64``). 81 | By default, this is published at 50Hz, however you specify the frequency through the ``status_hz`` parameter in the main configuration file. 82 | The message on the topic indicates whether the simulator is running or not. 83 | If it is running, then the value of the message is ``1``, otherwise it is ``0``. 84 | 85 | Time and ROS Clock 86 | ------------------ 87 | 88 | It is possible to syncronize the ROS clock time with the PyBullet simulation time. 89 | This means that when you call ``rospy.Time.now()`` in Python or ``ros::Time::now()`` in C++, the value will be equal to the simulation time in PyBullet. 90 | This can only be done when the ROS-Pybullet Interface node is configured to use manual time stepping (the reason for this is because `the Pybullet simulator time is not currently exposed `_). 91 | To synconize the PyBullet simulator time and ROS clock time, set the Boolean ROS parameter ``use_sim_time`` to ``true``. 92 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_robot_urdf.py: -------------------------------------------------------------------------------- 1 | import time 2 | from custom_ros_tools.config import ros_package_path, replace_package 3 | from .pybullet_object_pose import PybulletObjectPose 4 | 5 | class URDF: 6 | 7 | def __init__(self, pb_obj): 8 | self.pb_obj = pb_obj 9 | 10 | # If user specifies robot_description then get it from ros parameter 11 | if self.pb_obj.config['loadURDF']['fileName'] == 'robot_description': 12 | self.pb_obj.config['loadURDF']['fileName'] = self.get_urdf_from_robot_description() 13 | 14 | # Ensure urdf filename is an absolute path 15 | self.pb_obj.config['loadURDF']['fileName'] = replace_package(self.pb_obj.config['loadURDF']['fileName']) 16 | 17 | # Ensure no package:// statements appear in urdf (handle otherwise) 18 | if self.urdf_contains_ros_package_statements(): 19 | self.replace_ros_package_statements() 20 | 21 | # Parse loadURDF input 22 | if 'flags' in self.pb_obj.config['loadURDF']: 23 | self.pb_obj.config['loadURDF']['flags'] = self.pb_obj.node.parse_options(self.pb_obj.config['loadURDF']['flags']) 24 | 25 | # Create pose object 26 | self.pose = PybulletObjectPose(pb_obj) 27 | 28 | # Setup pose 29 | self.after_load_urdf_reset_base_velocity = False 30 | 31 | if self.is_fixed_base and self.pose.tf_specified(): 32 | # robot is fixed base and /tf is specified 33 | self.pose.start_reset_pose() # this can be used when the real robot base is defined by a /tf 34 | elif (not self.is_fixed_base) and (not self.pb_obj.is_visual_robot): 35 | # robot not fixed base and not visual (e.g. simulated quadruped) 36 | self.after_load_urdf_reset_base_velocity = True # inital velocity (set after pybullet.loadURDF is called in load method) 37 | elif (not self.is_fixed_base) and self.pb_obj.is_visual_robot and self.pose.tf_specified(): 38 | # robot is not fixed base, is visual, and tf specified (e.g. visual quadruped) 39 | self.pose.start_reset_pose() 40 | 41 | @property 42 | def is_fixed_base(self): 43 | return self.pb_obj.config['loadURDF'].get('useFixedBase', 0) 44 | 45 | @property 46 | def filename(self): 47 | return replace_package(self.pb_obj.config['loadURDF']['fileName']) 48 | 49 | @filename.setter 50 | def filename(self, value): 51 | self.pb_obj.config['loadURDF']['fileName'] = value 52 | 53 | def get_urdf_from_robot_description(self): 54 | 55 | # Get urdf string from ROS 56 | urdf_str = self.pb_obj.node.get_param('robot_description') 57 | 58 | # Dump urdf to temp file 59 | stamp = time.time_ns() # ensure filename uniqueness 60 | temp_filename = f'/tmp/pybullet_robot_urdf_robot_description_{stamp}.urdf' 61 | 62 | with open(temp_filename, 'w') as fout: 63 | fout.write(urdf_str) 64 | 65 | return temp_filename 66 | 67 | def user_given_base_position(self): 68 | return 'basePosition' in self.pb_obj.config['loadURDF'] 69 | 70 | def user_given_base_orientation(self): 71 | return 'baseOrientation' in self.pb_obj.config['loadURDF'] 72 | 73 | def urdf_contains_ros_package_statements(self): 74 | """Returns true when the URDF contains "package://" statements, false otherwise.""" 75 | with open(self.filename, 'r') as f: 76 | return 'package://' in f.read() 77 | 78 | def replace_ros_package_statements(self): 79 | """Replace "package://" statements with absolute paths in a new file.""" 80 | 81 | # Load urdf 82 | with open(self.filename, 'r') as fin: 83 | urdf_file_lines = fin.readlines() 84 | 85 | # Create new urdf in /tmp 86 | stamp = time.time_ns() # ensure filename uniqueness 87 | temp_filename = f'/tmp/pybullet_robot_urdf_{stamp}.urdf' 88 | with open(temp_filename, 'w') as fout: 89 | 90 | # Loop over urdf file lines 91 | for line in urdf_file_lines: 92 | 93 | # Check if package statement in line 94 | if 'package://' in line: 95 | # True -> replace package with absolute path 96 | idx = line.find("package://") 97 | package_name = line[idx:].split('/')[2] 98 | abs_path = ros_package_path(package_name) 99 | old = "package://" + package_name 100 | new = abs_path 101 | line_out = line.replace(old, new) 102 | else: 103 | # False -> no package statement, use line 104 | line_out = line 105 | 106 | # Write line to new temp file 107 | fout.write(line_out) 108 | 109 | self.filename = temp_filename 110 | 111 | def load(self): 112 | 113 | # Load URDF 114 | body_unique_id = self.pb_obj.pb.loadURDF(**self.pb_obj.config['loadURDF']) 115 | 116 | # If non fixed base and non visual robot -> allow user to set initial base velocity 117 | if self.after_load_urdf_reset_base_velocity: 118 | reset_base_velocity_input = self.pb_obj.config.get('resetBaseVelocity') 119 | if isinstance(reset_base_velocity_input, dict): 120 | self.pb_obj.pb.resetBaseVelocity(body_unique_id, **reset_base_velocity_input) 121 | elif reset_base_velocity_input is None: 122 | pass 123 | else: 124 | raise ValueError("resetBaseVelocity config type is not supported") 125 | 126 | return body_unique_id 127 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_visualizer.py: -------------------------------------------------------------------------------- 1 | from custom_ros_tools.config import load_config 2 | from sensor_msgs.msg import Image 3 | from cv_bridge import CvBridge 4 | from std_msgs.msg import Int64MultiArray 5 | import numpy as np 6 | from ros_pybullet_interface.msg import ResetDebugVisualizerCamera 7 | from ros_pybullet_interface.srv import GetDebugVisualizerCamera, GetDebugVisualizerCameraResponse 8 | 9 | class PybulletVisualizer: 10 | 11 | """Interface for the Pybullet visualizer.""" 12 | 13 | reset_debug_visualizer_camera_default = { 14 | 'cameraDistance': 1.0, 15 | 'cameraYaw': 0.0, 16 | 'cameraPitch': 1.0, 17 | 'cameraTargetPosition': [0.0]*3, 18 | } 19 | 20 | configure_debug_visualizer_default = dict( 21 | flag='COV_ENABLE_GUI', 22 | enable=0, 23 | ) 24 | 25 | def __init__(self, pb, node): 26 | 27 | # Set pybullet instance and ROS node 28 | self.pb = pb 29 | self.node = node 30 | 31 | # Initialize visualizer 32 | self.pb.configureDebugVisualizer(**self.configure_debug_visualizer) 33 | 34 | # Initialize camera pose 35 | self.pb.resetDebugVisualizerCamera(**self.reset_debug_visualizer_camera) 36 | 37 | # Setup subcscriber 38 | self.node.Subscriber('rpbi/reset_debug_visualizer_camera', ResetDebugVisualizerCamera, self.callback) 39 | 40 | self.pub_debug_vis = self.node.Publisher('rpbi/debug_visualizer_camera', ResetDebugVisualizerCamera, queue_size=10) 41 | self.node.Timer(self.node.Duration(1.0/30.0), self.debug_visualizer_camera_publish) 42 | 43 | 44 | # Setup publisher/timer for visualizer image 45 | self.cv_bridge = CvBridge() 46 | if self.publish_visualizer_image_hz > 0: 47 | self.visualizer_image_pub = self.node.Publisher( 48 | 'rpbi/visualizer_image', Image, queue_size=10 49 | ) 50 | self.visualizer_image_int_pub = self.node.Publisher( 51 | 'rpbi/visualizer_image_int', Int64MultiArray, queue_size=10 52 | ) 53 | dt = 1.0/float(self.publish_visualizer_image_hz) 54 | self.node.Timer(self.node.Duration(dt), self.publish_visualizer_image) 55 | 56 | # Setup service 57 | self.node.Service('rpbi/get_debug_visualizer_camera', GetDebugVisualizerCamera, self.service_get_debug_visualizer_camera) 58 | 59 | self.node.loginfo('initialized Pybullet visualizer') 60 | 61 | def debug_visualizer_camera_publish(self, event): 62 | msg = ResetDebugVisualizerCamera() 63 | for key, value in self.reset_debug_visualizer_camera.items(): 64 | setattr(msg, key, value) 65 | self.pub_debug_vis.publish(msg) 66 | 67 | @property 68 | def publish_visualizer_image_hz(self): 69 | return self.node.config.get('publish_visualizer_image_hz', 0) 70 | 71 | @property 72 | def visualizer_image_height(self): 73 | return self.node.config.get('visualizer_image_height', 480) 74 | 75 | @property 76 | def visualizer_image_width(self): 77 | return self.node.config.get('visualizer_image_width', 640) 78 | 79 | def publish_visualizer_image(self, event): 80 | 81 | # Get image 82 | _, _, colour, _, _ = self.pb.getCameraImage( 83 | self.visualizer_image_width, 84 | self.visualizer_image_height, 85 | renderer=self.pb.ER_BULLET_HARDWARE_OPENGL, 86 | ) 87 | 88 | self.visualizer_image_int_pub.publish(Int64MultiArray(data=colour.flatten().astype(int).tolist())) 89 | 90 | # Pack message 91 | msg = self.cv_bridge.cv2_to_imgmsg(colour[...,:3], encoding="rgb8") 92 | msg.header.stamp = self.node.time_now() 93 | 94 | # Publish image 95 | self.visualizer_image_pub.publish(msg) 96 | 97 | @property 98 | def configure_debug_visualizer(self): 99 | 100 | # Get from config 101 | configure_debug_visualizer = self.node.config.get('configureDebugVisualizer', {}) 102 | for key, default_value in self.configure_debug_visualizer_default.items(): 103 | if key not in configure_debug_visualizer: 104 | configure_debug_visualizer[key] = default_value 105 | 106 | # Handle flags 107 | if 'flag' in configure_debug_visualizer: 108 | configure_debug_visualizer['flag'] = self.node.parse_options(configure_debug_visualizer['flag']) 109 | 110 | return configure_debug_visualizer 111 | 112 | @property 113 | def reset_debug_visualizer_camera(self): 114 | reset_debug_visualizer_camera = self.node.config.get('resetDebugVisualizerCamera', {}) 115 | for key, default_value in self.reset_debug_visualizer_camera_default.items(): 116 | if key not in reset_debug_visualizer_camera: 117 | reset_debug_visualizer_camera[key] = default_value 118 | return reset_debug_visualizer_camera 119 | 120 | @reset_debug_visualizer_camera.setter 121 | def reset_debug_visualizer_camera(self, msg): 122 | assert isinstance(msg, ResetDebugVisualizerCamera), "reset_debug_visualizer_camera" 123 | if 'resetDebugVisualizerCamera' not in self.node.config: self.node.config['resetDebugVisualizerCamera'] = {} 124 | for key in self.reset_debug_visualizer_camera_default.keys(): 125 | self.node.config['resetDebugVisualizerCamera'][key] = getattr(msg, key) 126 | 127 | def service_get_debug_visualizer_camera(self, req): 128 | resp = GetDebugVisualizerCameraResponse() 129 | for key, value in self.reset_debug_visualizer_camera.items(): setattr(resp.debug_visualizer_config, key, value) 130 | return resp 131 | 132 | def callback(self, msg): 133 | """Callback for subscriber listening for visualizer pose updates.""" 134 | self.reset_debug_visualizer_camera = msg 135 | self.pb.resetDebugVisualizerCamera(**self.reset_debug_visualizer_camera) 136 | -------------------------------------------------------------------------------- /doc/examples.rst: -------------------------------------------------------------------------------- 1 | .. _examples: 2 | 3 | Examples 4 | ======== 5 | 6 | The examples for the ROS-PyBullet Interface are collected in a dedicated ROS package `rpbi_examples `_. 7 | The following gives details for each example and shows how to run them. 8 | 9 | Basic Examples 10 | -------------- 11 | 12 | .. code-block:: 13 | 14 | $ roslaunch basic_example_[NAME].launch 15 | 16 | The basic examples simply demonstrate the current robots that can be loaded into PyBullet out-of-the-box. 17 | Each example loads the given robot, and `a node `_ that generates a standardized motion on the robot. 18 | Some of the basic examples demonstrate different features of the library (e.g. recording a video, loading a URDF from the ROS parameter ``robot_description``). 19 | The following list links to the launch file for all the currently available basic examples. 20 | 21 | * `Kuka LWR `_ 22 | * `Talos `_ 23 | * `Kinova `_ 24 | * `Human model `_ 25 | * `Nextage `_ 26 | 27 | *Note*, the Kuka LWR example additionally demonstrates how to start recording videos and also how to attach a Force-Torque sensor to a robot joint. 28 | 29 | Human Interaction 30 | ----------------- 31 | 32 | .. image:: images/human_interaction.png 33 | :width: 600 34 | :alt: Alternative text 35 | 36 | .. code-block:: 37 | 38 | $ roslaunch rpbi_examples human_interaction.launch 39 | 40 | The goal of this example is to demonstrate how virtual forces, generated by the PyBullet simulator, can be rendered to the human via a haptic device. 41 | We have integrated the `3D Systems Touch X Haptic Device `_ into the framework, this is a necessary peice of hardware to run the example. 42 | Ensure you connect your device and that it is setup by following the instructions in the `RViMLab/geomagic_touch_x_ros `_ repository. 43 | 44 | When the example is launched, allow the haptic device to move to a nominal configuration. 45 | After it is calibrated and initialized you will be able to move the device up and down. 46 | The robot will track your movements in the z-axis until it reaches a block below the end-effector. 47 | The forces detected from a Force-Torque sensor, attached to the robot wrist, is rendered as feedback to the user - you will be able to feel the reaction forces through the device. 48 | 49 | Adding/removing PyBullet Objects Programmatically 50 | ------------------------------------------------- 51 | 52 | .. image:: images/pybullet_objects.png 53 | :width: 400 54 | :alt: Alternative text 55 | 56 | .. code-block:: 57 | 58 | $ roslaunch rpbi_examples pybullet_objects_example.launch 59 | 60 | Pybullet objects of all types can be specified in the launch file, or they can be added or removed programmatically (or even from the command line). 61 | This example demonstrates the ability of the ROS-PyBullet Interface to handle different objects. 62 | In this example, 63 | a collision object is loaded (the floor), 64 | a visual object is attached to a ``tf2`` frame moving in a figure-of-eight (the blue sphere), and 65 | a dynamic object is programmatically added and removed (the yellow box). 66 | 67 | Learning from demonstration and teleoperation 68 | --------------------------------------------- 69 | 70 | .. image:: images/kuka_push.png 71 | :width: 400 72 | :alt: Alternative text 73 | 74 | .. code-block:: 75 | 76 | $ roslaunch rpbi_examples lfd.launch 77 | 78 | In this example, we demonstrate how to easily connect the ROS-PyBullet Interface with an external ROS library. 79 | The goal of the task is for the robot to push the yellow box into the green goal. 80 | When the example is launched, the robot is initialized. 81 | You can interact with the demo using the keyboard - ensure the small window (the keyboard server) is in focus. 82 | Press *key 1* to send the robot to the initial position. 83 | Press *key 2* to start and stop teleoperation - when this is activated the robot states are being recorded (used as a demonstration to learn the DMP). 84 | Press *key 3* once to learn the DMP from the demonstration, and then again to plan and execute motion using the learned DMP. 85 | *Note*, the starting position for the DMP is always random. 86 | 87 | RGBD Sensor 88 | ----------- 89 | 90 | .. image:: images/rgbd_cloud.png 91 | :width: 400 92 | :alt: Alternative text 93 | 94 | .. code-block:: 95 | 96 | $ roslaunch rpbi_examples soft_body.launch 97 | 98 | In this example, we show how to setup an RGBD camera. 99 | This can be attached to any ``tf2`` frame, i.e. it could be attached to a robot link, for example. 100 | For this example, a similar scene is setup as in the pybullet objects example above. 101 | However, in addition, we include an RGBD camera where the camera orbits the scene. 102 | The projected point cloud is rendered in RVIZ as in the figure above. 103 | 104 | Soft bodies 105 | ----------- 106 | 107 | .. image:: images/soft.png 108 | :width: 400 109 | :alt: Alternative text 110 | 111 | .. code-block:: 112 | 113 | $ roslaunch rpbi_examples soft_body.launch 114 | 115 | This simple example demonstrates how soft bodies can be loaded into Pybullet. 116 | In addition, this example highlights how to load objects using the ``PybulletURDF`` object type - this is for loading objects (not robots) from a URDF file. 117 | The torus is a soft body, and the box and floor plane are loaded from URDF. 118 | It is also possible to load soft bodies from a URDF. 119 | 120 | 121 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_rgbd_sensor.py: -------------------------------------------------------------------------------- 1 | from std_msgs.msg import Header 2 | from sensor_msgs.msg import Image, CameraInfo, PointCloud2, PointField 3 | from sensor_msgs.point_cloud2 import create_cloud 4 | from .pybullet_sensor import PybulletSensor 5 | from cv_bridge import CvBridge 6 | import numpy as np 7 | import struct 8 | from .pybullet_object_pose import PybulletObjectPose 9 | from tf_conversions import transformations 10 | 11 | class PybulletRGBDSensor(PybulletSensor): 12 | 13 | def init(self): 14 | self.cv_bridge = CvBridge() 15 | 16 | self.cfg_camera = self.config.get('intrinsics', {}) 17 | 18 | self.pubs['colour'] = self.node.Publisher('rpbi/camera/colour/image', Image, queue_size=10) 19 | self.pubs['depth'] = self.node.Publisher('rpbi/camera/depth/image', Image, queue_size=10) 20 | self.pubs['ci/c'] = self.node.Publisher('rpbi/camera/colour/camera_info', CameraInfo, queue_size=10) 21 | self.pubs['ci/d'] = self.node.Publisher('rpbi/camera/depth/camera_info', CameraInfo, queue_size=10) 22 | 23 | self.pubs['segmentation'] = self.node.Publisher('rpbi/camera/segmentation', Image, queue_size=10) 24 | 25 | # publish point cloud? 26 | self.pub_pc = self.config.get("pointcloud", False) 27 | 28 | if self.pub_pc: 29 | self.pubs['pointcloud'] = self.node.Publisher('rpbi/camera/pointcloud', PointCloud2, queue_size=10) 30 | 31 | self.timers['mainloop'] = self.node.Timer(self.dt, self.main_loop) 32 | 33 | # TODO: expose other getCameraImage parameters so that user 34 | # can define view/projection matrix from a tf (if possible?) 35 | 36 | # intrinsics 37 | self.w = self.cfg_camera.get("width", 640) 38 | self.h = self.cfg_camera.get("height", 480) 39 | fov = self.cfg_camera.get("fov", 40) 40 | depth_range = self.cfg_camera.get("range", [0.01, 100]) 41 | self.near = depth_range[0] 42 | self.far = depth_range[1] 43 | self.pm = self.pb.computeProjectionMatrixFOV(fov = fov, aspect = self.w / self.h, nearVal = self.near, farVal = self.far) 44 | 45 | # bullet only supports a scalar field-of-view 46 | # determine the horizontal field-of-view from aspect ratio 47 | fovs = fov * np.array([self.w / self.h, 1]) 48 | # focal length in pixels 49 | self.fs = (0.5 * np.array([self.w, self.h])) / np.tan(np.deg2rad(fovs)/2) 50 | self.K = np.array([[self.fs[0], 0, self.w/2], 51 | [0, self.fs[1], self.h/2], 52 | [0, 0, 1 ]]) 53 | self.P = np.concatenate((self.K, np.zeros((3,1))), axis=1) 54 | 55 | if self.pub_pc: 56 | # precompute coordinates 57 | uv = np.dstack(np.meshgrid(range(self.w), range(self.h), indexing='xy')) 58 | uv_list = uv.reshape((-1,2)) 59 | # projection up to scale (scale with metric depth) 60 | xy = (uv_list - np.array([self.w/2, self.h/2])) / self.fs 61 | self.xy1 = np.concatenate((xy, np.ones((uv_list.shape[0],1))), axis=1) 62 | 63 | # Setup object pose 64 | self.pose = PybulletObjectPose(self) 65 | if not self.pose.tf_specified(): 66 | raise RuntimeError("an object_tf/tf_id must be specified for RGBD sensor") 67 | 68 | @property 69 | def dt(self): 70 | return self.node.Duration(1.0/float(self.config.get('hz', 30))) 71 | 72 | def main_loop(self, event): 73 | 74 | # extrinsics 75 | p, q = self.pose.get() 76 | T = transformations.translation_matrix(p) @ transformations.quaternion_matrix(q) 77 | # convert from OpenCV coordinate frame to OpenGL coordinate frame 78 | # rotate 180 deg about x-axis (have y and z point in the opposite direction) 79 | T[:,1:3] *= -1 80 | T = np.linalg.inv(T) 81 | # serialise column-wise 82 | vm = T.T.ravel() 83 | 84 | (width, height, colour, depth_gl, segmentation) = \ 85 | self.pb.getCameraImage(self.w, self.h, vm, self.pm, renderer=self.pb.ER_BULLET_HARDWARE_OPENGL) 86 | 87 | depth = self.far * self.near / (self.far - (self.far - self.near) * depth_gl) 88 | 89 | hdr = Header() 90 | hdr.stamp = self.node.time_now() 91 | # hdr.frame_id = 'rpbi/camera' 92 | hdr.frame_id = self.pose.tf_id 93 | 94 | # publish colour and depth image 95 | msg_colour = self.cv_bridge.cv2_to_imgmsg(colour[...,:3], encoding="rgb8") 96 | msg_colour.header = hdr 97 | msg_depth = self.cv_bridge.cv2_to_imgmsg(depth, encoding="passthrough") 98 | msg_depth.header = hdr 99 | 100 | self.pubs['colour'].publish(msg_colour) 101 | self.pubs['depth'].publish(msg_depth) 102 | 103 | # publish segmentation 104 | # NOTE: This will wrap around 16 bit unsigned integer (0 to 65,535) 105 | msg_segm = self.cv_bridge.cv2_to_imgmsg(segmentation.astype(np.uint16), encoding="mono16") 106 | msg_segm.header = hdr 107 | self.pubs['segmentation'].publish(msg_segm) 108 | 109 | # publish intrinsic parameters 110 | ci = CameraInfo() 111 | ci.header = hdr 112 | ci.width = width 113 | ci.height = height 114 | ci.K = self.K.ravel() 115 | ci.P = self.P.ravel() 116 | self.pubs['ci/c'].publish(ci) 117 | self.pubs['ci/d'].publish(ci) 118 | 119 | if self.pub_pc: 120 | # projection to 3D via precomputed coordinates 121 | points = self.xy1 * depth.reshape((-1,1)) 122 | # pack 3 x uint8 into float32 in order (0,r,g,b) 123 | rgb_f = np.empty((points.shape[0],1)) 124 | for i, rgb in enumerate(colour[...,:3].reshape((-1,3))): 125 | rgb_f[i] = struct.unpack('>f', struct.pack('4B', 0, *rgb))[0] 126 | 127 | fields = [PointField('x', 0, PointField.FLOAT32, 1), 128 | PointField('y', 4, PointField.FLOAT32, 1), 129 | PointField('z', 8, PointField.FLOAT32, 1), 130 | PointField('rgb', 12, PointField.FLOAT32, 1)] 131 | 132 | msg_pc = create_cloud(hdr, fields, np.concatenate((points, rgb_f), axis=1)) 133 | 134 | self.pubs['pointcloud'].publish(msg_pc) 135 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/ros_node.py: -------------------------------------------------------------------------------- 1 | import time 2 | import rospy 3 | from custom_ros_tools.tf import TfInterface 4 | 5 | """ 6 | 7 | ROS API 8 | 9 | The reason for this class is to minimize effort when parsing the 10 | ros_pybullet_interface to ROS2. 11 | 12 | A first draft for the RosNode implemented for ROS2 is at the end. 13 | 14 | When porting, consider notes in: 15 | https://github.com/cmower/ros_pybullet_interface/issues/102 16 | 17 | """ 18 | 19 | class RosNode: 20 | 21 | def __init__(self, *args, **kwargs): 22 | rospy.init_node(*args, **kwargs) 23 | self.tf = TfInterface() 24 | self.Time = rospy.Time 25 | 26 | def on_shutdown(self, *args, **kwargs): 27 | rospy.on_shutdown(*args, **kwargs) 28 | 29 | def time_now(self): 30 | return rospy.Time.now() 31 | 32 | def sleep(self, duration): 33 | rospy.sleep(duration) 34 | 35 | def ROSException(self, msg=''): 36 | rospy.exceptions.ROSException(msg) 37 | 38 | def wait_for_tf(self, parent_frame_id, child_frame_id, timeout=None): 39 | r = self.Rate(100) 40 | if timeout is not None: 41 | timeout_t = time.time() + timeout 42 | while True: 43 | pos, rot = self.tf.get_tf(parent_frame_id, child_frame_id) 44 | if pos is not None: 45 | break 46 | if time.time() >= timeout_t: 47 | raise self.ROSException("timeout exceeded while waiting for tf %s in %s" % (child_frame_id, parent_frame_id)) 48 | r.sleep() 49 | else: 50 | while True: 51 | pos, rot = self.tf.get_tf(parent_frame_id, child_frame_id) 52 | if pos is not None: 53 | break 54 | r.sleep() 55 | return pos, rot 56 | 57 | def wait_for_service(self, *args, **kwargs): 58 | rospy.wait_for_service(*args, **kwargs) 59 | 60 | def wait_for_message(self, *args, **kwargs): 61 | return rospy.wait_for_message(*args, **kwarg) 62 | 63 | def set_param(self, *args, **kwargs): 64 | return rospy.set_param(*args, **kwargs) 65 | 66 | def get_param(self, *args, **kwargs): 67 | return rospy.get_param(*args, **kwargs) 68 | 69 | def Publisher(self, *args, **kwargs): 70 | return rospy.Publisher(*args, **kwargs) 71 | 72 | def Subscriber(self, *args, **kwargs): 73 | return rospy.Subscriber(*args, **kwargs) 74 | 75 | def Service(self, *args, **kwargs): 76 | return rospy.Service(*args, **kwargs) 77 | 78 | def ServiceProxy(self, *args, **kwargs): 79 | return rospy.ServiceProxy(*args, **kwargs) 80 | 81 | def Timer(self, *args, **kwargs): 82 | return rospy.Timer(*args, **kwargs) 83 | 84 | def Rate(self, *args, **kwargs): 85 | return rospy.Rate(*args, **kwargs) 86 | 87 | def Duration(self, *args, **kwargs): 88 | return rospy.Duration(*args, **kwargs) 89 | 90 | def logdebug(self, *args, **kwargs): 91 | rospy.logdebug(*args, **kwargs) 92 | 93 | def loginfo(self, *args, **kwargs): 94 | rospy.loginfo(*args, **kwargs) 95 | 96 | def logwarn(self, *args, **kwargs): 97 | rospy.logwarn(*args, **kwargs) 98 | 99 | def logerr(self, *args, **kwargs): 100 | rospy.logerr(*args, **kwargs) 101 | 102 | def logfatal(self, *args, **kwargs): 103 | rospy.logfatal(*args, **kwargs) 104 | 105 | def spin(self): 106 | rospy.spin() 107 | 108 | 109 | 110 | # import rclpy 111 | # import inspect 112 | # from rclpy.node import Node 113 | # from functools import partial 114 | 115 | 116 | # class RosNode(Node): 117 | 118 | # def __init__(self, *args, **kwargs): 119 | # node_name = args[0] 120 | # super().__init__(node_name) 121 | # self.logger = self.get_logger() 122 | 123 | # def set_param(self, *args, **kwargs): 124 | # return rospy.set_param(*args, **kwargs) # TODO 125 | 126 | # def get_param(self, *args, **kwargs): 127 | # return rospy.get_param(*args, **kwargs) # TODO 128 | 129 | # def Publisher(self, *args, **kwargs): 130 | # msg_type = args[1] 131 | # topic = args[0] 132 | # qos_profile = kwargs['queue_size'] 133 | # return self.create_publisher(msg_type, topic, qos_profile) 134 | 135 | # def Subscriber(self, *args, **kwargs): 136 | # msg_type = args[1] 137 | # topic = args[0] 138 | # callback = args[2] 139 | # if 'callback_args' in kwargs: 140 | # partial_args = [callback] 141 | # callback_args_param_str = list(inspect.signature(callback).parameters.keys())[1] 142 | # partial_kwargs = {callback_args_param_str: kwargs['callback_args']} 143 | # callback_use = partial(*partial_args, **partial_kwargs) 144 | # else: 145 | # callback_use = callback 146 | # return self.create_subscription(msg_type, topic, callback_use) 147 | 148 | # def Service(self, *args, **kwargs): 149 | # srv_type = args[1] 150 | # srv_name = args[0] 151 | # callback = args[2] 152 | # return self.create_service(srv_type, srv_name, callback) 153 | 154 | # def Timer(self, *args, **kwargs): 155 | # timer_period_secs = args[0] 156 | # callback = args[1] 157 | # return self.create_timer(timer_period_secs, callback) 158 | 159 | # def Duration(self, *args, **kwargs): 160 | # return float(args[0]) 161 | 162 | # def _handle_log_args(self, *args): 163 | # if len(args) > 1: 164 | # message = args[0] % args[1:] 165 | # else: 166 | # message = args[0] 167 | # return message 168 | 169 | # def logdebug(self, *args, **kwargs): 170 | # self.logger.debug(self._handle_log_args(*args)) 171 | 172 | # def loginfo(self, *args, **kwargs): 173 | # self.logger.info(self._handle_log_args(*args)) 174 | 175 | # def logwarn(self, *args, **kwargs): 176 | # self.logger.warning(self._handle_log_args(*args)) 177 | 178 | # def logerr(self, *args, **kwargs): 179 | # self.logger.error(self._handle_log_args(*args)) 180 | 181 | # def logfatal(self, *args, **kwargs): 182 | # self.logger.fatal(self._handle_log_args(*args)) 183 | 184 | # def spin(self): 185 | # rclpy.spin(self) 186 | -------------------------------------------------------------------------------- /ros_pybullet_interface/src/rpbi/pybullet_robot.py: -------------------------------------------------------------------------------- 1 | from .pybullet_robot_joints import Joints 2 | from .pybullet_robot_links import Links 3 | from .pybullet_robot_ik import Ik 4 | from .pybullet_robot_urdf import URDF 5 | from .pybullet_object import PybulletObject 6 | 7 | from ros_pybullet_interface.srv import RobotInfo, RobotInfoResponse 8 | from ros_pybullet_interface.srv import ResetJointState, ResetJointStateResponse, ResetJointStateRequest 9 | from ros_pybullet_interface.srv import CalculateInverseKinematics, CalculateInverseKinematicsResponse 10 | from ros_pybullet_interface.srv import ResetEffState, ResetEffStateResponse 11 | 12 | class PybulletRobot(PybulletObject): 13 | 14 | """Interface for robots in Pybullet (simulated and visualizations). Note, visualized robots do not interact with objects in Pybullet.""" 15 | 16 | def init(self): 17 | 18 | # Setup robot 19 | self.urdf = URDF(self) 20 | self.body_unique_id = self.urdf.load() 21 | self.joints = Joints(self) 22 | self.links = Links(self, self.joints, self.urdf) 23 | self.ik = Ik(self, self.joints) 24 | 25 | # Handle robot with transparency 26 | if self.color_alpha is not None: 27 | for data in self.pb.getVisualShapeData(self.body_unique_id): 28 | link_index = data[1] 29 | rgba = data[7] 30 | new_rgba = (rgba[0], rgba[1], rgba[2], self.color_alpha) 31 | self.pb.changeVisualShape(self.body_unique_id, link_index, rgbaColor=new_rgba) 32 | 33 | # Setup services 34 | self.srvs['robot_info'] = self.node.Service(f'rpbi/{self.name}/robot_info', RobotInfo, self.service_robot_info) 35 | if not self.is_visual_robot: 36 | self.srvs['move_to_joint_state'] = self.node.Service(f'rpbi/{self.name}/move_to_joint_state', ResetJointState, self.service_move_to_joint_state) 37 | self.srvs['move_to_init_joint_state'] = self.node.Service(f'rpbi/{self.name}/move_to_initial_joint_state', ResetJointState, self.service_move_to_initial_joint_state) 38 | self.srvs['move_to_eff_state'] = self.node.Service(f'rpbi/{self.name}/move_to_eff_state', ResetEffState, self.service_move_eff_to_state) 39 | self.srvs['ik'] = self.node.Service(f'rpbi/{self.name}/ik', CalculateInverseKinematics, self.service_ik) 40 | 41 | # Set initial joint state 42 | if self.move_to_initial_joint_state_on_startup: 43 | self.service_move_to_initial_joint_state(ResetJointStateRequest(duration=0.01)) 44 | 45 | @property 46 | def move_to_initial_joint_state_on_startup(self): 47 | return self.config.get('move_to_initial_joint_state_on_startup', False) 48 | 49 | @property 50 | def color_alpha(self): 51 | return self.config.get('color_alpha', None) 52 | 53 | @property 54 | def is_visual_robot(self): 55 | return self.config.get('is_visual_robot', False) 56 | 57 | def service_robot_info(self, req): 58 | return RobotInfoResponse( 59 | robot_name=self.name, 60 | root_link_name=self.links.root_link_name, 61 | bodyUniqueId=self.body_unique_id, 62 | numJoints=self.joints.num_joints, 63 | numDof=self.joints.ndof, 64 | joint_info=[j.joint_info_msg for j in self.joints], 65 | enabled_ft_sensors=[j.jointName for j in self.joints if j.ft_sensor_enabled], 66 | current_joint_state=self.joints.get_current_joint_state_as_msg(), 67 | ) 68 | 69 | def service_move_to_initial_joint_state(self, req): 70 | req.joint_state = self.joints.initial_joint_state 71 | return self.service_move_to_joint_state(req) 72 | 73 | def service_move_to_joint_state(self, req): 74 | 75 | if self.joints.control_mode not in {self.pb.POSITION_CONTROL, self.pb.VELOCITY_CONTROL}: 76 | success = False 77 | message = f'robot in {self.joints.control_mode} control mode, currently this service only supports POSITION_CONTROL/VELOCITY_CONTROL modes!' 78 | self.node.logerr(message) 79 | return ResetJointStateResponse(message=message, success=success) 80 | 81 | # Setup 82 | success = True 83 | message = 'successfully moved robot to joint state' 84 | 85 | # Move joints 86 | try: 87 | self.joints.move_to_joint_state(req.joint_state, req.duration) 88 | except Exception as err: 89 | success = False 90 | message = 'failed to move robot to target joint state, exception: ' + str(err) 91 | 92 | if success: 93 | self.node.loginfo(message) 94 | else: 95 | self.node.logerr(message) 96 | 97 | return ResetJointStateResponse(message=message, success=success) 98 | 99 | def service_ik(self, req): 100 | resp = CalculateInverseKinematicsResponse(success=True, message='solved ik') 101 | try: 102 | resp.solution = self.ik.solve(req.problem) 103 | except Exception as err: 104 | resp.success = False 105 | resp.message = 'failed to solve ik, exception: ' + str(err) 106 | if resp.success: 107 | self.node.loginfo(resp.message) 108 | else: 109 | self.node.logerr(resp.message) 110 | return resp 111 | 112 | def service_move_eff_to_state(self, req): 113 | 114 | # Setup 115 | success = True 116 | message = 'moved robot to target eff position' 117 | 118 | # Solve ik 119 | try: 120 | target_joint_state = self.ik.solve(req.problem) 121 | except Exception as err: 122 | success = False 123 | message = 'failed to solve ik, exception: ' + str(err) 124 | self.node.logerr(message) 125 | return ResetEffStateResponse(success=success, message=message) 126 | 127 | # Move robot 128 | try: 129 | self.joints.move_to_joint_state(target_joint_state, req.duration) 130 | except Exception as err: 131 | success = False 132 | message = 'failed to move robot to target eff state, exception: ' + str(err) 133 | self.node.logerr(message) 134 | return ResetEffStateResponse(success=success, message=message) 135 | 136 | self.node.loginfo(message) 137 | return ResetEffStateResponse(success=success, message=message) 138 | -------------------------------------------------------------------------------- /rpbi_examples/scripts/run_interpolation.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | import sys 3 | import rospy 4 | import numpy as np 5 | import matplotlib.pyplot as plt 6 | 7 | from custom_ros_tools.tf import TfInterface 8 | from std_msgs.msg import Float64MultiArray 9 | from std_msgs.msg import MultiArrayDimension 10 | 11 | 12 | class Node: 13 | 14 | def __init__(self): 15 | rospy.init_node('run_interpolation_node') 16 | self.tf = TfInterface() 17 | 18 | # initialize empty trajectory plan 19 | self.traj_plan = np.empty(0) 20 | 21 | # topic where sequence of waypoints is published 22 | wpts_topic_name = rospy.get_param( 23 | '~topic_name_4_waypoints', 'ros_pybullet_interface/waypt_traj') 24 | 25 | # init the publisher 26 | self.new_traj_publisher = rospy.Publisher( 27 | f"{wpts_topic_name}", Float64MultiArray, queue_size=1) 28 | 29 | # create two lists for storing and plotting reasons 30 | self.actual_motion = [] 31 | self.interpol_plan = [] 32 | self.time_of_motion = [] 33 | self.init_time = 0.0 34 | 35 | def publish_trajectory(self, event): 36 | 37 | message = self.np2D_to_ROSmsg(self.traj_plan) 38 | 39 | if message is not None: 40 | self.new_traj_publisher.publish(message) 41 | 42 | def np2D_to_ROSmsg(self, data2D_array): 43 | 44 | # check if there is nothing to publish 45 | if data2D_array.size == 0: 46 | return 47 | 48 | r, c = data2D_array.shape 49 | 50 | # info: http://docs.ros.org/en/api/std_msgs/html/msg/MultiArrayLayout.html 51 | # Pack trajectory msg 52 | msg = Float64MultiArray() 53 | 54 | # specify that the array has 2 dimensions 55 | msg.layout.dim.append(MultiArrayDimension()) 56 | msg.layout.dim.append(MultiArrayDimension()) 57 | 58 | # info for reconstruction of the 2D array 59 | msg.layout.dim[0].label = "rows" 60 | msg.layout.dim[0].size = r 61 | msg.layout.dim[1].label = "columns" 62 | msg.layout.dim[1].size = c 63 | 64 | # add data as flattened numpy array 65 | msg.data = data2D_array.flatten('C') # row major flattening 66 | 67 | # time is missing from this message 68 | # msg.header.stamp = rospy.Time.now() 69 | return msg 70 | 71 | def exec_plan(self): 72 | 73 | # Generate plan 74 | rospy.loginfo('generating plan by interpolating between waypoints...') 75 | 76 | # Execute plan 77 | rospy.loginfo('generating waypoints ...') 78 | 79 | # get current TF of robot end effector 80 | init_eff_pos, init_eff_rot = self.tf.wait_for_tf( 81 | 'rpbi/world', 'rpbi/kuka_lwr/end_effector_ball') 82 | 83 | # 84 | # Manually create a sequence of waypoints 85 | # 86 | 87 | # make the time axis of the waypoints 88 | timeSeq = np.array([0.0, 4.0, 9.0, 14.0]) 89 | 90 | # initial position of the robot 91 | init_pose = np.hstack((init_eff_pos, init_eff_rot)) 92 | 93 | # corresponds to green sphere 94 | wpt1_pos = np.array([-0.4, -0.0, 0.1]) 95 | wpt1 = np.hstack((wpt1_pos, init_eff_rot)) 96 | 97 | # corresponds to blue sphere 98 | wpt2_pos = np.array([-0.5, 0.2, 0.3]) 99 | wpt2 = np.hstack((wpt2_pos, init_eff_rot)) 100 | 101 | # corresponds to red sphere 102 | wpt3_pos = np.array([-0.15, 0.4, 0.2]) 103 | wpt3 = np.hstack((wpt3_pos, init_eff_rot)) 104 | 105 | # generate the position of the waypoints 106 | pos_body = np.vstack((init_pose, wpt1)) 107 | pos_body = np.vstack((pos_body, wpt2)) 108 | pos_body = np.vstack((pos_body, wpt3)).T 109 | 110 | # generate a rough velocity of the waypoints 111 | diff = np.diff(pos_body, axis=1) 112 | velBody = pos_body*0.0 113 | scale_vel_factor = 0.5 114 | velBody[:3, 1:-1] = scale_vel_factor*(diff[:3, :-1] + diff[:3, 1:])/2.0 115 | 116 | # assemble the waypoint plan 117 | self.traj_plan = np.vstack((np.vstack((timeSeq, pos_body)), velBody)) 118 | 119 | # publish the waypoint plan 120 | self.write_callback_timer = rospy.Timer( 121 | rospy.Duration(1.0/float(2)), self.publish_trajectory) 122 | 123 | rospy.loginfo("Waypoints were published!") 124 | 125 | def collect_interpol_plan_and_actual_motion_data(self): 126 | 127 | interpol_eff_pos, _ = self.tf.get_tf('rpbi/kuka_lwr/lwr_arm_0_link', 'interpol_target') 128 | curr_eff_pos, _ = self.tf.get_tf('rpbi/world', 'rpbi/kuka_lwr/end_effector_ball') 129 | 130 | if interpol_eff_pos is not None: 131 | self.interpol_plan.append(interpol_eff_pos) 132 | self.actual_motion.append(curr_eff_pos) 133 | self.time_of_motion.append(rospy.Time.now().to_sec() - self.init_time) 134 | else: 135 | self.init_time = rospy.Time.now().to_sec() 136 | 137 | def plot_results(self): 138 | 139 | # 140 | interpol_plan = np.asarray(self.interpol_plan) 141 | actual_motion = np.asarray(self.actual_motion) 142 | 143 | time_of_motion = self.time_of_motion 144 | 145 | fig = plt.figure() 146 | ax1 = plt.subplot(1, 3, 1) 147 | ax1.plot(self.traj_plan[0, :], self.traj_plan[1, :], '*', markersize=14, label='Waypts') 148 | ax1.plot(time_of_motion, interpol_plan[:, 0], 'b', label='Inpterpolated plan') 149 | ax1.plot(time_of_motion, actual_motion[:, 0], 'r', label='Actual motion') 150 | ax1.legend(loc="upper left") 151 | ax1.set_title("X axis") 152 | 153 | ax2 = plt.subplot(1, 3, 2) 154 | ax2.plot(self.traj_plan[0, :], self.traj_plan[2, :], '*', markersize=14) 155 | ax2.plot(time_of_motion, interpol_plan[:, 1], 'b') 156 | ax2.plot(time_of_motion, actual_motion[:, 1], 'r') 157 | ax2.set_title("Y axis") 158 | 159 | ax3 = plt.subplot(1, 3, 3) 160 | ax3.plot(self.traj_plan[0, :], self.traj_plan[3, :], '*', markersize=14) 161 | ax3.plot(time_of_motion, interpol_plan[:, 2], 'b') 162 | ax3.plot(time_of_motion, actual_motion[:, 2], 'r') 163 | ax3.set_title("Z axis") 164 | 165 | fig.suptitle("Linear axes interpolation results", fontsize=16) 166 | plt.show() 167 | 168 | def spin(self): 169 | rate = rospy.Rate(100.0) 170 | while not rospy.is_shutdown(): 171 | self.collect_interpol_plan_and_actual_motion_data() 172 | rate.sleep() 173 | self.plot_results() 174 | 175 | 176 | def main(): 177 | node = Node() 178 | node.exec_plan() 179 | node.spin() 180 | 181 | 182 | if __name__ == '__main__': 183 | main() 184 | --------------------------------------------------------------------------------