├── .github └── ISSUE_TEMPLATE │ └── bug_report.md ├── .rosinstall.melodic ├── .rosinstall.noetic ├── .travis.sh ├── .travis.yml ├── README.md ├── cart_humanoid ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── config │ ├── cart_humanoid_controllers.yaml │ ├── click_ik.rviz │ └── sample_head_arm_skin_detect.rviz ├── euslisp │ ├── cart_humanoid-interface.l │ ├── jaxon_red.l │ └── realtime-ik-sample.l ├── images │ └── cart_humanoid_gazebo.png ├── launch │ ├── cart_humanoid_gazebo.launch │ ├── click_ik_rviz.launch │ └── sample_head_arm_skin_detect.launch ├── package.xml ├── scripts │ ├── sample_julius_cmd.py │ ├── skin_ellipse_area_to_ik_tgt.py │ └── tuck_arm.py ├── urdf │ ├── JAXON_RED.urdf.xacro │ ├── JAXON_RED_body_stable_inertia.urdf │ ├── JAXON_RED_meshes │ │ ├── BODY_mesh.dae │ │ ├── CHEST_LINK0_mesh.dae │ │ ├── CHEST_LINK1_mesh.dae │ │ ├── CHEST_LINK2_mesh.dae │ │ ├── HEAD_LINK0_mesh.dae │ │ ├── HEAD_LINK1_mesh.dae │ │ ├── LARM_LINK0_mesh.dae │ │ ├── LARM_LINK1_mesh.dae │ │ ├── LARM_LINK2_mesh.dae │ │ ├── LARM_LINK3_mesh.dae │ │ ├── LARM_LINK4_mesh.dae │ │ ├── LARM_LINK5_mesh.dae │ │ ├── LARM_LINK6_mesh.dae │ │ ├── LARM_LINK7_mesh.dae │ │ ├── LLEG_LINK0_mesh.dae │ │ ├── LLEG_LINK1_mesh.dae │ │ ├── LLEG_LINK2_mesh.dae │ │ ├── LLEG_LINK3_mesh.dae │ │ ├── LLEG_LINK4_mesh.dae │ │ ├── LLEG_LINK5_mesh.dae │ │ ├── RARM_LINK0_mesh.dae │ │ ├── RARM_LINK1_mesh.dae │ │ ├── RARM_LINK2_mesh.dae │ │ ├── RARM_LINK3_mesh.dae │ │ ├── RARM_LINK4_mesh.dae │ │ ├── RARM_LINK5_mesh.dae │ │ ├── RARM_LINK6_mesh.dae │ │ ├── RARM_LINK7_mesh.dae │ │ ├── RLEG_LINK0_mesh.dae │ │ ├── RLEG_LINK1_mesh.dae │ │ ├── RLEG_LINK2_mesh.dae │ │ ├── RLEG_LINK3_mesh.dae │ │ ├── RLEG_LINK4_mesh.dae │ │ ├── RLEG_LINK5_mesh.dae │ │ ├── l_hand_attached_link.dae │ │ └── r_hand_attached_link.dae │ ├── hand.urdf.xacro │ ├── hand_meshes │ │ ├── JVRC_finger1.stl │ │ ├── JVRC_finger2.stl │ │ └── JVRC_palm.stl │ ├── my_mesh.stl │ ├── my_model.urdf.xacro │ └── robot.urdf.xacro └── worlds │ ├── humanoid_workspace.world │ └── model │ └── my_model │ ├── model.config │ ├── model.sdf │ └── my_mesh.stl ├── daisya_euslisp_tutorials ├── CHANGELOG.rst ├── CMakeLists.txt ├── euslisp │ ├── daisya-hanoi.l │ ├── daisya-ik.l │ ├── daisya-maze-2011.l │ ├── daisya-maze.l │ ├── daisya-qlearn.l │ ├── daisya-subsumption.l │ ├── daisya.l │ ├── gsearch.l │ ├── hanoi.l │ ├── hsearch.l │ ├── irteusext.l │ ├── maze.l │ ├── psolve.l │ ├── qlearn.l │ ├── robot-server-simulator.l │ ├── sample-robot-server.bak.l │ ├── sample-robot-server.l │ ├── sample-sim-real.l │ ├── sample-sim.l │ ├── sets.l │ ├── subsumption.l │ └── virtual_interpolator.l ├── mainpage.dox ├── package.xml └── test │ ├── test-daisya-ik.l │ ├── test-daisya-ik.test │ ├── test-daisya-maze.l │ ├── test-daisya-maze.test │ ├── test-daisya.l │ ├── test-daisya.test │ ├── test-maze.l │ ├── test-maze.test │ ├── test-psolve.l │ ├── test-psolve.test │ ├── test-sample-robot-server.bak.l │ ├── test-sample-robot-server.bak.launch │ ├── test-sample-robot-server.l │ ├── test-sample-robot-server.launch │ ├── test-subsumption.l │ └── test-subsumption.test ├── dxl_armed_turtlebot ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── config │ ├── dxl_armed_turtlebot.rviz │ ├── dxl_armed_turtlebot_controllers.yaml │ ├── map.pgm │ └── map.yaml ├── doc │ ├── Makefile │ ├── robot_programming_manual.tex │ ├── robot_programming_model.l │ └── robot_programming_ri.l ├── euslisp │ ├── arm-move-sample.l │ ├── display-bounding-box-array.l │ ├── display-checkerboard.l │ ├── dxl-armed-turtlebot-interface.l │ ├── dxl-armed-turtlebot.l │ ├── gazebo-grasp-test.l │ ├── gazebo-grasp-with-recognition.l │ ├── joy-sample.l │ ├── keyboard-ik-sample.l │ └── realtime-ik-sample.l ├── images │ ├── gazebo_checkerboard.png │ ├── gazebo_turtlebot2.png │ ├── gazebo_turtlebot_grasp.png │ └── gazebo_turtlebot_slam.png ├── kadai │ ├── checkpoint1-3-1-button.l │ ├── checkpoint1-3-2-led.l │ ├── checkpoint1_3_1_button.py │ ├── checkpoint1_3_2_led.py │ ├── checkpoint2-1-go-forward.l │ ├── checkpoint2-3-joy-ik.l │ ├── kadai2_3_cmd_vel.py │ └── kadai2_3_joint_command.py ├── launch │ ├── checkerboard_detector.launch │ ├── dxl_armed_turtlebot_bringup.launch │ ├── dxl_armed_turtlebot_gazebo.launch │ ├── dxl_armed_turtlebot_navigation.launch │ ├── enshu.perspective │ ├── hsi_color_filter.launch │ ├── includes │ │ └── amcl.launch.xml │ ├── sample_head_arm_skin_detect.launch │ └── turtlebot_joystick_teleop.launch ├── mainpage.dox ├── package.xml ├── scripts │ ├── install_ps3joy_daemon.sh │ ├── install_turtlebot_simulator.sh │ ├── ps3joy │ └── tuck_arm.py ├── test │ ├── test-dxl-armed-turtlebot.l │ └── test-dxl-armed-turtlebot.test ├── urdf │ └── robot.urdf.xacro └── worlds │ ├── empty.world │ └── model │ └── checkerboard │ ├── checkerboard_20mm_7x4.dae │ ├── checkerboard_20mm_7x4.urdf │ ├── checkerboard_20mm_7x4_color.png │ ├── model.config │ └── model.sdf ├── dynamixel_7dof_arm ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ └── dynamixel_joint_controllers.yaml ├── euslisp │ ├── dxl-7dof-arm-interface-common.l │ ├── dxl-7dof-arm-interface.l │ ├── dxl-7dof-arm-robot.l │ ├── old_examples │ │ ├── dxl-7dof-arm-robot-ver1.l │ │ └── test-dynamixel-motor.l │ └── thermo-speaker.l ├── launch │ ├── dynamixel_7dof_arm_bringup.launch │ ├── gripper_controller_spawner.launch │ └── soundplay.launch ├── mainpage.dox ├── package.xml ├── scripts │ ├── 58-dynamixel_configurator.rules │ ├── create_udev_rules │ ├── gripper_controller_spawner.sh │ ├── reset_torque.py │ └── scan_ids.py ├── test │ ├── test-dxl-7dof-arm.l │ └── test-dxl-7dof-arm.test └── urdf │ ├── dynamixel_7dof_arm.urdf.xacro │ └── meshes │ ├── F2.stl │ ├── ax12.stl │ └── dxl_gripper.stl ├── mechatrobot ├── .clang-format ├── .gitignore ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ ├── controllers.yaml │ └── robot.rviz ├── euslisp │ └── motor-command-by-face.l ├── launch │ ├── mechatrobot_controller.launch │ ├── mechatrobot_display.launch │ ├── mechatrobot_driver.launch │ └── sample_face_detect.launch ├── package.xml ├── scripts │ └── motor-command-by-face.py ├── sketchbook │ ├── MechatrobotDriver │ │ └── MechatrobotDriver.ino │ ├── dynamixel_motor_sample │ │ ├── dynamixel_motor_sample.ino │ │ └── src │ │ │ └── DynamixelAX12A.h │ ├── led_sample │ │ └── led_sample.ino │ ├── ros_dynamixel_motor_sample │ │ ├── ros_dynamixel_motor_sample.ino │ │ └── src │ ├── stepping_motor_sample_28BYJ48 │ │ └── stepping_motor_sample_28BYJ48.ino │ └── ultrasonic_sensor_sample │ │ └── ultrasonic_sensor_sample.ino ├── src │ └── robot_control.cpp └── urdf │ ├── 28BYJ-48.STL │ ├── HC-SR04.STL │ ├── robot.urdf │ └── sample-3d-model.stl └── turtleboteus ├── CHANGELOG.rst ├── CMakeLists.txt ├── euslisp ├── turtlebot-interface-common.l ├── turtlebot-interface.l └── turtlebot-with-sensors-robot.l ├── models ├── kobuki_hexagons_kinect.dae ├── kobuki_hexagons_kinect.l └── kobuki_hexagons_kinect.urdf ├── package.xml └── test ├── test-turtlebot.l └── test-turtlebot.test /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: Bug report 3 | about: Create a report to help us improve 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | **Describe the bug** 11 | A clear and concise description of what the bug is. 12 | 13 | **To Reproduce** 14 | Steps to reproduce the behavior: 15 | 1. Go to '...' 16 | 2. Click on '....' 17 | 3. Scroll down to '....' 18 | 4. See error 19 | 20 | **Expected behavior** 21 | A clear and concise description of what you expected to happen. 22 | 23 | **Screenshots** 24 | If applicable, add screenshots to help explain your problem. 25 | 26 | **Desktop (please complete the following information):** 27 | - OS: [e.g. iOS] 28 | - Browser [e.g. chrome, safari] 29 | - Version [e.g. 22] 30 | 31 | **Additional context** 32 | Add any other context about the problem here. 33 | -------------------------------------------------------------------------------- /.rosinstall.melodic: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: robot-programming 3 | uri: https://github.com/jsk-enshu/robot-programming.git 4 | - git: 5 | local-name: dynamixel_urdf 6 | uri: https://github.com/jsk-enshu/dynamixel_urdf.git 7 | - git: 8 | local-name: dynamixel_motor 9 | uri: https://github.com/arebgun/dynamixel_motor.git 10 | - git: 11 | local-name: kobuki 12 | uri: https://github.com/yujinrobot/kobuki.git 13 | version: 0.7.6 14 | - git: 15 | local-name: kobuki_desktop 16 | uri: https://github.com/yujinrobot/kobuki_desktop.git 17 | version: 58459065087c7f9786cc2d1dc25a059280b5ca47 18 | # - git: 19 | # local-name: turtlebot 20 | # uri: https://github.com/turtlebot/turtlebot.git 21 | # version: feb176b10f13bcdc517e7f305c39327dac6388f0 # this has not been testd on real robot, fix xtion visual? or back to 2019 version? 22 | - git: 23 | local-name: turtlebot 24 | uri: https://github.com/kindsenior/turtlebot.git 25 | version: use-freenect-feb176b # fix for launching 3dsensor.launch in students' PC until https://github.com/turtlebot/turtlebot/pull/294 will be merged 26 | - git: 27 | local-name: turtlebot_simulator 28 | uri: https://github.com/turtlebot/turtlebot_simulator.git 29 | version: beae4b3cdf806fe82ab75fd933189c67e89ed8b0 # this has not been testd on real robot, fix tf warning? or back to 2017 version 30 | - git: 31 | local-name: turtlebot_apps 32 | uri: https://github.com/turtlebot/turtlebot_apps.git 33 | version: 4bbafc7bfdfc5888ba03b52611bdb56d86b22967 34 | - git: 35 | local-name: turtlebot_interactions 36 | uri: https://github.com/turtlebot/turtlebot_interactions.git 37 | version: 2.3.1 38 | - git: 39 | local-name: turtlebot_msgs 40 | uri: https://github.com/turtlebot/turtlebot_msgs.git 41 | version: 2.2.1 42 | -------------------------------------------------------------------------------- /.rosinstall.noetic: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: robot-programming 3 | uri: https://github.com/jsk-enshu/robot-programming.git 4 | - git: 5 | local-name: dynamixel_urdf 6 | uri: https://github.com/jsk-enshu/dynamixel_urdf.git 7 | - git: 8 | local-name: dynamixel_motor 9 | # uri: https://github.com/arebgun/dynamixel_motor.git 10 | uri: https://github.com/kindsenior/dynamixel_motor.git 11 | version: noetic-support-python3 # need until https://github.com/arebgun/dynamixel_motor/pull/104 wil be merged 12 | - git: 13 | local-name: kobuki 14 | uri: https://github.com/yujinrobot/kobuki.git 15 | version: melodic 16 | - git: 17 | local-name: kobuki_desktop 18 | # uri: https://github.com/yujinrobot/kobuki_desktop.git 19 | # version: melodic 20 | uri: https://github.com/kindsenior/kobuki_desktop.git 21 | version: remove-repeated-tf-publication # need until https://github.com/yujinrobot/kobuki_desktop/pull/69 wil be merged 22 | - git: 23 | local-name: turtlebot 24 | uri: https://github.com/turtlebot/turtlebot.git 25 | version: melodic 26 | - git: 27 | local-name: turtlebot_simulator 28 | uri: https://github.com/turtlebot/turtlebot_simulator.git 29 | version: melodic 30 | - git: 31 | local-name: turtlebot_apps 32 | uri: https://github.com/turtlebot/turtlebot_apps.git 33 | version: indigo 34 | - git: 35 | local-name: turtlebot_interactions 36 | uri: https://github.com/turtlebot/turtlebot_interactions.git 37 | version: indigo 38 | - git: 39 | local-name: turtlebot_msgs 40 | uri: https://github.com/turtlebot/turtlebot_msgs.git 41 | version: indigo 42 | - git: 43 | local-name: yujin_ocs 44 | # uri: https://github.com/yujinrobot/yujin_ocs.git 45 | # version: devel # add CATKIN_IGNORE in packages except for cmd_vel_mux, controllers, math_toolkit, velocity_smoother, and virtual_sensor 46 | uri: https://github.com/kindsenior/yujin_ocs.git 47 | version: noetic-enshu2023 # for ignoring those packages 48 | - git: 49 | local-name: ar_track_alvar_msgs 50 | uri: https://github.com/ros-gbp/ar_track_alvar-release.git 51 | version: release/melodic/ar_track_alvar_msgs/0.7.1-0 # generated by `rosinstall_generator ar_track_alvar_msgs --rosdistro melodic` 52 | - git: 53 | local-name: joystick_drivers 54 | uri: https://github.com/ros-drivers/joystick_drivers.git 55 | version: 1.15.0 # Last version before removing ps3joy 56 | - git: 57 | local-name: yocs_msgs 58 | uri: https://github.com/yujinrobot/yocs_msgs.git 59 | version: devel # until release of ros-noetic-yocs-msgs 60 | # - git: 61 | # local-name: orocos_kinematics_dynamics 62 | # uri: https://github.com/orocos/orocos_kinematics_dynamics.git 63 | # version: master # until release of ros-noetic-orocos-kdl 64 | - git: 65 | local-name: std_capabilities 66 | uri: https://github.com/osrf/std_capabilities.git 67 | version: master # until release of ros-noetic-std-capabilities 68 | # - git: 69 | # local-name: jsk_pr2eus 70 | # uri: https://github.com/jsk-ros-pkg/jsk_pr2eus.git 71 | # version: master # add CATKIN_IGNORE in packages except for pr2eus 72 | - git: 73 | local-name: jsk_roseus # for disabling warning of tf dupulications 74 | # uri: https://github.com/jsk-ros-pkg/jsk_roseus.git 75 | uri: https://github.com/k-okada/jsk_roseus.git 76 | version: set-log-level-to-error 77 | - git: 78 | local-name: ros_controllers 79 | uri: https://github.com/ros-controls/ros_controllers.git 80 | version: 0.20.0 # after this version, ros_control does not publish /motor1/position based on /position_trajectory_controller/command from /rqt_joint_trajectory_controller because of the same warning https://github.com/ros-controls/ros_controllers/issues/291 -------------------------------------------------------------------------------- /.travis.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | set -x 4 | set -e 5 | 6 | apt-get update 7 | apt-get install -y sudo software-properties-common git wget sed 8 | 9 | # fix stopping tzdata for 18.04 10 | echo 'debconf debconf/frontend select Noninteractive' | sudo debconf-set-selections 11 | 12 | echo "Testing branch $TRAVIS_BRANCH of $REPOSITORY_NAME" 13 | sudo sh -c "echo \"deb ${REPOSITORY} `lsb_release -cs` main\" > /etc/apt/sources.list.d/ros-latest.list" 14 | wget http://packages.ros.org/ros.key -O - | sudo apt-key add - 15 | sudo apt-get update -qq 16 | sudo apt-get install -qq -y python-rosdep python-catkin-tools python-wstool ros-${ROS_DISTRO}-catkin 17 | sudo rosdep init 18 | rosdep update --include-eol-distros 19 | # script: 20 | (cd ${CI_SOURCE_PATH}; git log --oneline | head -10) 21 | mkdir -p ~/catkin_ws/src 22 | cd ~/catkin_ws 23 | ln -sf ${CI_SOURCE_PATH} src/${REPOSITORY_NAME} 24 | wstool init src 25 | [ -e src/${REPOSITORY_NAME}/.rosinstall.${ROS_DISTRO} ] && wstool merge -t src src/${REPOSITORY_NAME}/.rosinstall.${ROS_DISTRO} 26 | wstool update -t src --continue-on-error 27 | rosdep install --from-paths src -y -r -q --ignore-src --rosdistro ${ROS_DISTRO} 28 | source /opt/ros/${ROS_DISTRO}/setup.bash 29 | env | grep ROS 30 | rosversion catkin 31 | # Build 32 | catkin build -v -i --summarize --no-status -p 1 -j 1 --no-notify # every 5 mim 33 | # Run tests 34 | catkin run_tests 35 | # check test (this only works on indigo) 36 | catkin_test_results --verbose build 37 | 38 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: trusty 3 | language: generic 4 | env: 5 | matrix: 6 | - ROS_DISTRO="kinetic" REPOSITORY=http://packages.ros.org/ros/ubuntu 7 | - ROS_DISTRO="kinetic" REPOSITORY=http://packages.ros.org/ros-testing/ubuntu 8 | - ROS_DISTRO="melodic" REPOSITORY=http://packages.ros.org/ros/ubuntu 9 | - ROS_DISTRO="melodic" REPOSITORY=http://packages.ros.org/ros-testing/ubuntu 10 | # matrix: 11 | # allow_failures: 12 | # - env: ROS_DISTRO="melodic" REPOSITORY=http://packages.ros.org/ros/ubuntu 13 | install: 14 | - case $ROS_DISTRO in "hydro") export DISTRO=precise;; "indigo") export DISTRO=trusty;; "kinetic") export DISTRO=xenial;; "melodic") export DISTRO=bionic;; esac; 15 | - export DOCKER_IMAGE=ubuntu:$DISTRO 16 | - export CI_SOURCE_PATH=$(pwd) 17 | - export REPOSITORY_NAME=${PWD##*/} 18 | - docker images 19 | - docker run -v $HOME:$HOME -e CI_SOURCE_PATH -e REPOSITORY_NAME -e HOME -e DISTRO -e ROS_DISTRO -e REPOSITORY $DOCKER_IMAGE bash -c 'cd $CI_SOURCE_PATH; source .travis.sh' 20 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | robot-programming [![Build Status](https://app.travis-ci.com/jsk-enshu/robot-programming.svg?branch=master)](https://app.travis-ci.com/jsk-enshu/robot-programming) [![Join the chat at https://gitter.im/jsk-enshu/robot-programming](https://badges.gitter.im/Join%20Chat.svg)](https://gitter.im/jsk-enshu/robot-programming?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge) 2 | ================= 3 | 4 | This is exercise for robot-programming. 5 | 6 | # Setup 7 | ----- 8 | 9 | ``` 10 | $ source /opt/ros/noetic/setup.bash 11 | $ mkdir -p ~/catkin_ws/src 12 | $ cd ~/catkin_ws/src 13 | $ git clone https://github.com/jsk-enshu/robot-programming 14 | $ wstool init . 15 | $ wstool merge robot-programming/.rosinstall.${ROS_DISTRO} 16 | $ wstool update 17 | $ rosdep update 18 | $ cd .. 19 | $ rosdep install --from-paths src --ignore-src -y -r 20 | # You will get a warning because some packages (turtlebot_bringup, turtlebot_dashboard, turtlebot_rapps, and kobuki_capabilities) have not resolved the rosdep key, but you can ignore it. 21 | $ catkin build 22 | # $ echo 'source ~/catkin_ws/devel/setup.bash' >> ~/.bashrc ## You may source the setup.bash by this line if you understand the difference between > and >>. 23 | ``` 24 | 25 | # Turtlebot simulation 26 | ## Start simulator 27 | --------------- 28 | ``` 29 | $ source ~/catkin_ws/devel/setup.bash 30 | $ roslaunch dxl_armed_turtlebot dxl_armed_turtlebot_gazebo.launch 31 | ``` 32 | 33 | ## Start RQT GUI 34 | ------------- 35 | ``` 36 | $ roscd dxl_armed_turtlebot/launch 37 | 38 | $ rqt --perspective-file enshu.perspective 39 | ``` 40 | 41 | ## Start Color Tracking node 42 | ------------------------- 43 | ``` 44 | $ roslaunch opencv_apps camshift.launch image:=/camera/rgb/image_raw 45 | $ rosrun image_view2 image_view2 image:=/camera/rgb/image_raw ~image_transport:=compressed 46 | ``` 47 | 48 | ## Start Checkerboard Tracking Tracking node 49 | ----------------------------------------- 50 | ``` 51 | $ roslaunch roseus_tutorials checkerboard-detector.launch rect0_size_x:=0.02 rect0_size_y:=0.02 \ 52 | grid0_size_x:=7 grid0_size_y:=4 translation0:="0 0 0" \ 53 | image:=image_raw group:=/camera/rgb frame_id:=camera_rgb_optical_frame 54 | $ ROS_NAMESPACE=/camera/rgb rosrun checkerboard_detector objectdetection_tf_publisher.py \ 55 | _use_simple_tf:=true 56 | ``` 57 | 58 | # cart_humanoid (JAXON) simulation 59 | ![cart humanoid](./cart_humanoid/images/cart_humanoid_gazebo.png) 60 | 61 | ## Start simulator 62 | --------------- 63 | ``` 64 | $ source ~/catkin_ws/devel/setup.bash 65 | $ roslaunch cart_humanoid cart_humanoid_gazebo.launch 66 | # It may take a few minutes to start the simulator for the first time. 67 | # If you launch simulators with the robot's base link unfixed, use the following command instead. 68 | $ roslaunch cart_humanoid cart_humanoid_gazebo.launch fix_base_link:=false 69 | ``` 70 | 71 | ## Start RQT GUI 72 | ------------- 73 | ``` 74 | $ rosrun rqt_joint_trajectory_controller rqt_joint_trajectory_controller 75 | ``` 76 | 77 | # Documentations 78 | ---------------- 79 | 80 | 81 | See online [manual](http://jsk-enshu.github.io/robot-programming/) for Euslisp models and interfaces. 82 | 83 | PDF files are also available from [here](http://jsk-enshu.github.io/robot-programming/robot_programming_manual.pdf) 84 | 85 | -------------------------------------------------------------------------------- /cart_humanoid/.gitignore: -------------------------------------------------------------------------------- 1 | *~ 2 | \#*\# 3 | .\#* -------------------------------------------------------------------------------- /cart_humanoid/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package cart_humanoid 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 7.0.0 (2020-11-24) 6 | ------------------ 7 | * fix invalid version number in package.xml 8 | * Merge pull request #386 from ishiguroJSK/2020 9 | add sample julius node 10 | * add sample julius node 11 | * Merge branch 'master' into arm-test 12 | * Merge branch 'master' into fix_camera_name 13 | * Merge branch 'master' into mechatrobot 14 | * Merge pull request #370 from ishiguroJSK/2020 15 | 演習2020 2日目 16 | * add missing run_depend 17 | * Merge branch '2020' of https://github.com/ishiguroJSK/robot-programming into 2020 18 | * add start-grasp 19 | * Merge branch 'master' of https://github.com/jsk-enshu/robot-programming into mechatrobot 20 | * Merge branch 'master' into mechatrobot 21 | * Merge pull request #332 from ishiguroJSK/2020 22 | [WIP] 2020 演習 23 | * update package.xml 24 | * up 25 | * add cart_humanoid 26 | * Contributors: Kei Okada, Yuki Asano, ishiguroJSK 27 | 28 | 6.1.0 (2019-11-11) 29 | ------------------ 30 | 31 | 6.0.0 (2019-06-16) 32 | ------------------ 33 | 34 | 5.0.0 (2018-09-14) 35 | ------------------ 36 | 37 | 4.0.1 (2017-11-20) 38 | ------------------ 39 | 40 | 4.0.0 (2017-09-24) 41 | ------------------ 42 | 43 | 3.2.2 (2016-12-01) 44 | ------------------ 45 | 46 | 3.2.1 (2016-11-30) 47 | ------------------ 48 | 49 | 3.2.0 (2016-11-29) 50 | ------------------ 51 | 52 | 3.1.0 (2016-11-07) 53 | ------------------ 54 | 55 | 3.0.1 (2016-11-06) 56 | ------------------ 57 | 58 | 3.0.0 (2016-10-16) 59 | ------------------ 60 | 61 | 2.1.5 (2015-11-25) 62 | ------------------ 63 | 64 | 2.1.4 (2015-11-24) 65 | ------------------ 66 | 67 | 2.1.3 (2015-11-19) 68 | ------------------ 69 | 70 | 2.1.2 (2015-11-12) 71 | ------------------ 72 | 73 | 2.1.1 (2015-11-11 12:55) 74 | ------------------------ 75 | 76 | 2.1.0 (2015-11-11 03:25) 77 | ------------------------ 78 | 79 | 2.0.0 (2015-11-10) 80 | ------------------ 81 | 82 | 1.0.3 (2015-11-09) 83 | ------------------ 84 | 85 | 1.0.2 (2014-12-02) 86 | ------------------ 87 | 88 | 1.0.1 (2014-11-27) 89 | ------------------ 90 | 91 | 1.0.0 (2014-11-11) 92 | ------------------ 93 | -------------------------------------------------------------------------------- /cart_humanoid/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(cart_humanoid) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roseus) # add roseus to gen messages 5 | 6 | catkin_package() 7 | 8 | # if(CATKIN_ENABLE_TESTING) 9 | # find_package(catkin REQUIRED COMPONENTS rostest roslaunch) 10 | # # roslacunch check 11 | # file(GLOB LAUNCH_FILES launch/*.launch) 12 | # foreach(LAUNCH_FILE ${LAUNCH_FILES}) 13 | # if("${LAUNCH_FILE}" STREQUAL "${CMAKE_CURRENT_SOURCE_DIR}/launch/dxl_armed_turtlebot_bringup.launch") 14 | # find_package(freenect_launch QUIET) 15 | # if(NOT freenect_launch_FOUND) 16 | # continue() 17 | # endif() 18 | # endif() 19 | # if("${LAUNCH_FILE}" STREQUAL "${CMAKE_CURRENT_SOURCE_DIR}/launch/checkerboard_detector_5x4.launch") 20 | # # to avoid error: "ROSLaunch config error: unrecognized tag sphinxdoc" 21 | # # https://github.com/jsk-ros-pkg/jsk_roseus/blob/master/roseus_tutorials/launch/checkerboard-detector.launch#L35 22 | # continue() 23 | # endif() 24 | # roslaunch_add_file_check(${LAUNCH_FILE}) 25 | # endforeach() 26 | # # test 27 | # find_package(pr2eus QUIET) 28 | # if(pr2eus_FOUND) 29 | # add_rostest(test/test-dxl-armed-turtlebot.test) 30 | # endif() 31 | # endif() 32 | -------------------------------------------------------------------------------- /cart_humanoid/README.md: -------------------------------------------------------------------------------- 1 | roslaunch cart_humanoid cart_humanoid_gazebo.launch 2 | -------------------------------------------------------------------------------- /cart_humanoid/config/cart_humanoid_controllers.yaml: -------------------------------------------------------------------------------- 1 | # Joint State Controllers --------------------------------------- 2 | joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 50 5 | # Trajectory Controllers --------------------------------------- 6 | fullbody_controller: 7 | type: effort_controllers/JointTrajectoryController 8 | joints: 9 | - LLEG_JOINT0 10 | - LLEG_JOINT1 11 | - LLEG_JOINT2 12 | - LLEG_JOINT3 13 | - LLEG_JOINT4 14 | - LLEG_JOINT5 15 | 16 | - RLEG_JOINT0 17 | - RLEG_JOINT1 18 | - RLEG_JOINT2 19 | - RLEG_JOINT3 20 | - RLEG_JOINT4 21 | - RLEG_JOINT5 22 | 23 | - LARM_JOINT0 24 | - LARM_JOINT1 25 | - LARM_JOINT2 26 | - LARM_JOINT3 27 | - LARM_JOINT4 28 | - LARM_JOINT5 29 | - LARM_JOINT6 30 | - LARM_JOINT7 31 | 32 | - RARM_JOINT0 33 | - RARM_JOINT1 34 | - RARM_JOINT2 35 | - RARM_JOINT3 36 | - RARM_JOINT4 37 | - RARM_JOINT5 38 | - RARM_JOINT6 39 | - RARM_JOINT7 40 | 41 | - CHEST_JOINT0 42 | - CHEST_JOINT1 43 | - CHEST_JOINT2 44 | 45 | - HEAD_JOINT0 46 | - HEAD_JOINT1 47 | 48 | gains: # Required because we're controlling an effort interface 49 | LLEG_JOINT0: {p: 10000, i: 0, d: 100} 50 | LLEG_JOINT1: {p: 10000, i: 0, d: 100} 51 | LLEG_JOINT2: {p: 10000, i: 0, d: 100} 52 | LLEG_JOINT3: {p: 1000, i: 0, d: 10} 53 | LLEG_JOINT4: {p: 100, i: 0, d: 1} 54 | LLEG_JOINT5: {p: 100, i: 0, d: 1} 55 | 56 | RLEG_JOINT0: {p: 10000, i: 0, d: 100} 57 | RLEG_JOINT1: {p: 10000, i: 0, d: 100} 58 | RLEG_JOINT2: {p: 10000, i: 0, d: 100} 59 | RLEG_JOINT3: {p: 1000, i: 0, d: 10} 60 | RLEG_JOINT4: {p: 100, i: 0, d: 1} 61 | RLEG_JOINT5: {p: 100, i: 0, d: 1} 62 | 63 | LARM_JOINT0: {p: 10000, i: 0, d: 100} 64 | LARM_JOINT1: {p: 10000, i: 0, d: 100} 65 | LARM_JOINT2: {p: 10000, i: 0, d: 100} 66 | LARM_JOINT3: {p: 10000, i: 0, d: 100} 67 | LARM_JOINT4: {p: 10000, i: 0, d: 100} 68 | LARM_JOINT5: {p: 1000, i: 0, d: 10} 69 | LARM_JOINT6: {p: 1000, i: 0, d: 10} 70 | LARM_JOINT7: {p: 1000, i: 0, d: 10} 71 | 72 | RARM_JOINT0: {p: 10000, i: 0, d: 100} 73 | RARM_JOINT1: {p: 10000, i: 0, d: 100} 74 | RARM_JOINT2: {p: 10000, i: 0, d: 100} 75 | RARM_JOINT3: {p: 10000, i: 0, d: 100} 76 | RARM_JOINT4: {p: 10000, i: 0, d: 100} 77 | RARM_JOINT5: {p: 1000, i: 0, d: 10} 78 | RARM_JOINT6: {p: 1000, i: 0, d: 10} 79 | RARM_JOINT7: {p: 1000, i: 0, d: 10} 80 | 81 | CHEST_JOINT0: {p: 10000, i: 0, d: 100} 82 | CHEST_JOINT1: {p: 10000, i: 0, d: 100} 83 | CHEST_JOINT2: {p: 10000, i: 0, d: 100} 84 | 85 | HEAD_JOINT0: {p: 100, i: 0, d: 1} 86 | HEAD_JOINT1: {p: 100, i: 0, d: 1} 87 | 88 | state_publish_rate: 100 # Override default 89 | action_monitor_rate: 100 # Override default 90 | stop_trajectory_duration: 0 # Override default 91 | 92 | l_gripper_controller: 93 | type: effort_controllers/JointTrajectoryController 94 | joints: 95 | - L_finger1_joint 96 | gains: 97 | L_finger1_joint: {p: 10, i: 0, d: 1} 98 | state_publish_rate: 100 # Override default 99 | action_monitor_rate: 100 # Override default 100 | stop_trajectory_duration: 0 # Override default 101 | 102 | r_gripper_controller: 103 | type: effort_controllers/JointTrajectoryController 104 | joints: 105 | - R_finger1_joint 106 | gains: 107 | R_finger1_joint: {p: 10, i: 0, d: 1} 108 | state_publish_rate: 100 # Override default 109 | action_monitor_rate: 100 # Override default 110 | stop_trajectory_duration: 0 # Override default 111 | 112 | # l_gripper_controller: 113 | # type: effort_controllers/JointPositionController 114 | # joint: L_finger1_joint 115 | # pid: {p: 10, i: 0, d: 1} 116 | 117 | # r_gripper_controller: 118 | # type: effort_controllers/JointPositionController 119 | # joint: R_finger1_joint 120 | # pid: {p: 10, i: 0, d: 1} -------------------------------------------------------------------------------- /cart_humanoid/euslisp/realtime-ik-sample.l: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env roseus 2 | (load "package://cart_humanoid/euslisp/cart_humanoid-interface.l") 3 | 4 | (ros::roseus "cart_humanoid_ik_node") 5 | 6 | (cart_humanoid-init) 7 | (setq *robot* *cart_humanoid*) 8 | 9 | (setq larm-tgt (make-coords :pos #f(500 300 0))) 10 | (setq rarm-tgt (make-coords :pos #f(500 -300 0))) 11 | (setq head-tgt (make-coords :pos #f(500 0 0))) 12 | (setq lleg-tgt (make-coords :pos #f( 0 100 -970))) 13 | (setq rleg-tgt (make-coords :pos #f( 0 -100 -970))) 14 | 15 | (objects *robot*) 16 | 17 | (defclass humanoid-ik-class 18 | :super propertied-object 19 | :slots ()) 20 | (defmethod humanoid-ik-class 21 | (:init 22 | () 23 | (ros::subscribe "ik_larm_tgt" geometry_msgs::PoseStamped #'send self :larm-cb) 24 | (ros::subscribe "ik_rarm_tgt" geometry_msgs::PoseStamped #'send self :rarm-cb) 25 | (ros::subscribe "ik_lleg_tgt" geometry_msgs::PoseStamped #'send self :lleg-cb) 26 | (ros::subscribe "ik_rleg_tgt" geometry_msgs::PoseStamped #'send self :rleg-cb) 27 | (ros::subscribe "ik_head_tgt" geometry_msgs::PoseStamped #'send self :head-cb) 28 | ) 29 | (:larm-cb 30 | (msg) 31 | (setq larm-tgt (ros::tf-pose->coords (send msg :pose)))) 32 | 33 | (:rarm-cb 34 | (msg) 35 | (setq rarm-tgt (ros::tf-pose->coords (send msg :pose)))) 36 | 37 | (:lleg-cb 38 | (msg) 39 | (setq lleg-tgt (ros::tf-pose->coords (send msg :pose)))) 40 | 41 | (:rleg-cb 42 | (msg) 43 | (setq rleg-tgt (ros::tf-pose->coords (send msg :pose)))) 44 | 45 | (:head-cb 46 | (msg) 47 | (setq head-tgt (ros::tf-pose->coords (send msg :pose)))) 48 | 49 | ) 50 | 51 | 52 | (setq m (instance humanoid-ik-class :init)) 53 | 54 | (defun run () 55 | (send *ri* :angle-vector (send *robot* :reset-floating-pose) 2000) 56 | (send *ri* :wait-interpolation) 57 | (send *ri* :angle-vector (send *robot* :reset-pose) 2000) 58 | (send *ri* :wait-interpolation) 59 | 60 | (ros::rate 10) 61 | (while (ros::ok) 62 | (send *robot* :inverse-kinematics (list larm-tgt rarm-tgt lleg-tgt rleg-tgt) 63 | :move-target (list 64 | (send *robot* :larm :end-coords) 65 | (send *robot* :rarm :end-coords) 66 | (send *robot* :lleg :end-coords) 67 | (send *robot* :rleg :end-coords)) 68 | :translation-axis (list t t t t) 69 | :rotation-axis (list t t t t) 70 | :stop 2 71 | :revert-if-fail nil :debug-view nil 72 | ) 73 | (send *robot* :look-at-target head-tgt) 74 | (send *ri* :angle-vector (send *robot* :angle-vector) 10) 75 | (send *irtviewer* :draw-objects :flush nil) 76 | (dolist (tgt (list larm-tgt rarm-tgt lleg-tgt rleg-tgt head-tgt)) (send tgt :draw-on :flush nil)) 77 | (send *irtviewer* :viewer :viewsurface :redraw) 78 | (x::window-main-one) 79 | (ros::spin-once) 80 | (ros::sleep) 81 | ) 82 | ) 83 | 84 | (run) 85 | 86 | 87 | -------------------------------------------------------------------------------- /cart_humanoid/images/cart_humanoid_gazebo.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/cart_humanoid/images/cart_humanoid_gazebo.png -------------------------------------------------------------------------------- /cart_humanoid/launch/cart_humanoid_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 18 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 36 | 37 | 38 | 39 | 40 | 41 | 44 | 45 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 80 | 81 | 82 | 83 | 84 | -------------------------------------------------------------------------------- /cart_humanoid/launch/click_ik_rviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /cart_humanoid/launch/sample_head_arm_skin_detect.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 | -------------------------------------------------------------------------------- /cart_humanoid/package.xml: -------------------------------------------------------------------------------- 1 | 2 | cart_humanoid 3 | 7.0.0 4 | Humanoid robot gazebo simulation with fixed base link 5 | BSD 6 | Kei Okada 7 | Kei Okada 8 | 9 | catkin 10 | 11 | 12 | 13 | roseus 14 | rostest 15 | rqt_joint_trajectory_controller 16 | teleop_twist_keyboard 17 | 18 | controller_manager 19 | effort_controllers 20 | pr2_mechanism_model 21 | depthimage_to_laserscan 22 | 23 | gazebo_plugins 24 | gazebo_ros 25 | gazebo_ros_control 26 | image_view2 27 | joint_state_controller 28 | joint_trajectory_controller 29 | joy 30 | jsk_perception 31 | jsk_pcl_ros 32 | jsk_tools 33 | ps3joy 34 | 35 | 36 | 37 | 38 | map_server 39 | nodelet 40 | opencv_apps 41 | 42 | robot_state_publisher 43 | rqt_reconfigure 44 | rviz 45 | topic_tools 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | rqt_joint_trajectory_controller 55 | rqt_robot_steering 56 | teleop_twist_keyboard 57 | roseus_tutorials 58 | slam_karto 59 | roseus 60 | openni_launch 61 | usb_cam 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /cart_humanoid/scripts/sample_julius_cmd.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # coding=utf-8 3 | import rospy 4 | from geometry_msgs.msg import * 5 | from speech_recognition_msgs.msg import * 6 | from speech_recognition_msgs.srv import * 7 | 8 | class sample_julius_cmd: 9 | 10 | def __init__(self): 11 | rospy.init_node("sample_julius_cmd") 12 | rospy.Subscriber("/speech_to_text", SpeechRecognitionCandidates, self.cb) 13 | self.pub = rospy.Publisher("/cmd_vel_mux/input/teleop", Twist, queue_size=1) 14 | 15 | rospy.loginfo("Setting up vocabulary...") 16 | rospy.wait_for_service('/speech_recognition') 17 | try: 18 | change_vocabulary = rospy.ServiceProxy('/speech_recognition', SpeechRecognition) 19 | req = SpeechRecognitionRequest() 20 | req.vocabulary.words = ["みぎ","ひだり","まえ","うしろ","だんす"] 21 | req.quiet = True 22 | res = change_vocabulary(req) 23 | print res 24 | except rospy.ServiceException, e: 25 | print "Service call failed: %s"%e 26 | rospy.loginfo("OK") 27 | rospy.spin() 28 | 29 | def cb(self, msg): 30 | rospy.loginfo(msg.transcript[0]) 31 | vel = Twist() 32 | 33 | if msg.transcript[0] == "まえ": 34 | vel.linear.x = 0.2 35 | 36 | if msg.transcript[0] == "うしろ": 37 | vel.linear.x = -0.2 38 | 39 | if msg.transcript[0] == "みぎ": 40 | vel.angular.z = -1 41 | 42 | if msg.transcript[0] == "ひだり": 43 | vel.angular.z = 1 44 | 45 | self.pub.publish(vel) 46 | 47 | 48 | if __name__ == '__main__': 49 | instance = sample_julius_cmd() 50 | -------------------------------------------------------------------------------- /cart_humanoid/scripts/skin_ellipse_area_to_ik_tgt.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # coding=utf-8 3 | import rospy 4 | from sensor_msgs.msg import * 5 | from geometry_msgs.msg import * 6 | from opencv_apps.msg import * 7 | 8 | class skin_ellipse_area_to_ik_tgt: 9 | 10 | def __init__(self): 11 | rospy.init_node("skin_ellipse_area_to_ik_tgt") 12 | 13 | # get params from roslaunch 14 | self.frame_id = rospy.get_param('~frame_id') 15 | self.const_depth = rospy.get_param('~const_depth') 16 | 17 | rospy.Subscriber("/head_arm/general_contours/ellipses", RotatedRectArrayStamped, self.cb) 18 | self.head_pub = rospy.Publisher("/ik_head_tgt", PoseStamped, queue_size=1) 19 | self.larm_pub = rospy.Publisher("/ik_larm_tgt", PoseStamped, queue_size=1) 20 | self.rarm_pub = rospy.Publisher("/ik_rarm_tgt", PoseStamped, queue_size=1) 21 | # self.lleg_pub = rospy.Publisher("/ik_lleg_tgt", PoseStamped, queue_size=1) 22 | # self.rleg_pub = rospy.Publisher("/ik_rleg_tgt", PoseStamped, queue_size=1) 23 | rospy.spin() 24 | 25 | def cb(self, msg): 26 | # Structure of RotatedRectArrayStamped (rostopic echo) 27 | # rects: 28 | # - 29 | # angle: 0.0 30 | # center: 31 | # x: 0.0 32 | # y: 0.0 33 | # size: 34 | # width: 0.0 35 | # height: 0.0 36 | # - 37 | # angle: 114.636924744 38 | # center: 39 | # x: 376.963684082 40 | # y: 404.353729248 41 | # size: 42 | # width: 3.78912639618 43 | # height: 4.34580373764 44 | 45 | unsorted_list = [ (rs.size.width * rs.size.height, rs.center.x, rs.center.y) for rs in msg.rects] 46 | top3_area_list = sorted([(area, x, y) for (area, x, y) in unsorted_list], reverse=True)[:3] 47 | pos_sorted_list = sorted([(x, y, area) for (area, x, y) in top3_area_list]) 48 | # # sorted by X pos in image coords, and simple labeling 49 | # # left-most ellipse in the image = right hand 50 | # # middle ellipse in the image = face 51 | # # right-most ellipse in the image = left hand 52 | 53 | valid_area_min = 10000 54 | if len([area for (x, y, area) in pos_sorted_list if area > valid_area_min]) < 3: 55 | rospy.logwarn_throttle(1, "valid skin area < 3, keep waiting...") 56 | return 57 | else: 58 | rarm_pos = pos_sorted_list[0] 59 | head_pos = pos_sorted_list[1] 60 | larm_pos = pos_sorted_list[2] 61 | 62 | rospy.loginfo("larm: " + str(larm_pos) + " head: " + str(head_pos) + " rarm: " + str(rarm_pos)) 63 | 64 | # 2D image coords [0~640, 0~480] -> 3D robot coords [CONST_DEPTH, -0.5~0.5, 0~1] 65 | IMAGE_W = 640.0 # [pixel] 66 | IMAGE_H = 480.0 # [pixel] 67 | CONST_DEPTH = self.const_depth # for 2D -> 3D [] 68 | val = PoseStamped() 69 | val.pose.orientation.w = 1.0 70 | 71 | val.header.frame_id = self.frame_id 72 | val.pose.position.x = CONST_DEPTH 73 | val.pose.position.y = (head_pos[0] - (IMAGE_W/2)) / (IMAGE_W/2) * 0.5 74 | val.pose.position.z = -(head_pos[1] - (IMAGE_H/2)) / (IMAGE_H/2) * 0.5 + 0.5 75 | self.head_pub.publish(val) 76 | 77 | val.header.frame_id = self.frame_id 78 | val.pose.position.x = CONST_DEPTH 79 | val.pose.position.y = (larm_pos[0] - (IMAGE_W/2)) / (IMAGE_W/2) * 0.5 80 | val.pose.position.z = -(larm_pos[1] - (IMAGE_H/2)) / (IMAGE_H/2) * 0.5 + 0.5 81 | self.larm_pub.publish(val) 82 | 83 | val.header.frame_id = self.frame_id 84 | val.pose.position.x = CONST_DEPTH 85 | val.pose.position.y = (rarm_pos[0] - (IMAGE_W/2)) / (IMAGE_W/2) * 0.5 86 | val.pose.position.z = -(rarm_pos[1] - (IMAGE_H/2)) / (IMAGE_H/2) * 0.5 + 0.5 87 | self.rarm_pub.publish(val) 88 | 89 | 90 | if __name__ == '__main__': 91 | instance = skin_ellipse_area_to_ik_tgt() 92 | -------------------------------------------------------------------------------- /cart_humanoid/scripts/tuck_arm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | 4 | import rospy 5 | import argparse 6 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 7 | from control_msgs.msg import JointTrajectoryControllerState 8 | from math import pi 9 | 10 | class Tuck(object): 11 | def __init__(self, tuck_cmd): 12 | self._done = False 13 | self._tuck = tuck_cmd 14 | self._tuck_rate = rospy.Rate(5.0) # Hz 15 | self._tuck_threshold = 0.3 # radians 16 | self._joint_moves = { 17 | 'tuck': [0]*33, 18 | 'untuck': [0]*33, 19 | } 20 | self._arm_state = 'none' 21 | self._pub = rospy.Publisher('/fullbody_controller/command', JointTrajectory, queue_size=1) 22 | self._sub = rospy.Subscriber('/fullbody_controller/state', JointTrajectoryControllerState, self._check_arm_state) 23 | 24 | def _check_arm_state(self, msg): 25 | diff_check = lambda a, b: abs(a - b) <= self._tuck_threshold 26 | angles = msg.actual.positions 27 | tuck_goal = map(diff_check, angles, self._joint_moves['tuck']) 28 | untuck_goal = map(diff_check, angles, self._joint_moves['untuck']) 29 | 30 | if all(tuck_goal): 31 | self._arm_state = 'tuck' 32 | elif all(untuck_goal): 33 | self._arm_state = 'untuck' 34 | else: 35 | self._arm_state = 'none' 36 | 37 | def _prepare_to_tuck(self): 38 | pass 39 | 40 | def _move_to(self, goal): 41 | traj = JointTrajectory() 42 | traj.joint_names = \ 43 | ['LLEG_JOINT0', 'LLEG_JOINT1', 'LLEG_JOINT2', 'LLEG_JOINT3', 'LLEG_JOINT4', 'LLEG_JOINT5', 'RLEG_JOINT0',\ 44 | 'RLEG_JOINT1', 'RLEG_JOINT2', 'RLEG_JOINT3', 'RLEG_JOINT4', 'RLEG_JOINT5', 'LARM_JOINT0', 'LARM_JOINT1',\ 45 | 'LARM_JOINT2', 'LARM_JOINT3', 'LARM_JOINT4', 'LARM_JOINT5', 'LARM_JOINT6', 'LARM_JOINT7', 'RARM_JOINT0',\ 46 | 'RARM_JOINT1', 'RARM_JOINT2', 'RARM_JOINT3', 'RARM_JOINT4', 'RARM_JOINT5', 'RARM_JOINT6', 'RARM_JOINT7',\ 47 | 'CHEST_JOINT0', 'CHEST_JOINT1', 'CHEST_JOINT2', 'HEAD_JOINT0', 'HEAD_JOINT1'] 48 | traj.points.append(JointTrajectoryPoint()) 49 | traj.points[0].positions = self._joint_moves[goal] 50 | traj.points[0].time_from_start = rospy.Duration(3) 51 | 52 | while self._arm_state != goal: 53 | self._pub.publish(traj) 54 | self._tuck_rate.sleep() 55 | 56 | # self._pub.publish(traj) 57 | 58 | def supervised_tuck(self): 59 | # Update our starting state to check if arms are tucked 60 | self._prepare_to_tuck() 61 | # Tuck Arms 62 | if self._tuck == True: 63 | # If arms are already tucked, report this to user and exit. 64 | if self._arm_state == 'tuck': 65 | rospy.loginfo("Tucking: Arm already in 'Tucked' position.") 66 | else: 67 | rospy.loginfo("Tucking: arm is not Tucked.") 68 | self._move_to('tuck') 69 | self._done = True 70 | return 71 | 72 | # Untuck Arms 73 | else: 74 | # If arms already untucked 75 | if self._arm_state == 'untuck': 76 | rospy.loginfo("Untucking: Arm already Untucked") 77 | else: 78 | rospy.loginfo("Untucking: arm is not Untucked.") 79 | self._move_to('untuck') 80 | self._done = True 81 | return 82 | 83 | def clean_shutdown(self): 84 | if not self._done: 85 | rospy.logwarn('Aborting: Shutting down safely...') 86 | 87 | 88 | def main(): 89 | parser = argparse.ArgumentParser() 90 | tuck_group = parser.add_mutually_exclusive_group(required=True) 91 | tuck_group.add_argument("-t", "--tuck", dest="tuck", 92 | action='store_true', default=False, help="tuck arms") 93 | tuck_group.add_argument("-u", "--untuck", dest="untuck", 94 | action='store_true', default=False, help="untuck arms") 95 | args = parser.parse_args(rospy.myargv()[1:]) 96 | tuck = args.tuck 97 | 98 | rospy.loginfo("Initializing node... ") 99 | rospy.init_node("tuck_arms") 100 | rospy.loginfo("%sucking arms" % ("T" if tuck else "Unt",)) 101 | tucker = Tuck(tuck) 102 | rospy.on_shutdown(tucker.clean_shutdown) 103 | # tucker.supervised_tuck() 104 | tucker._move_to('tuck') 105 | rospy.loginfo("Finished %suck" % ("T" if tuck else "Unt",)) 106 | 107 | if __name__ == "__main__": 108 | main() 109 | -------------------------------------------------------------------------------- /cart_humanoid/urdf/hand.urdf.xacro: -------------------------------------------------------------------------------- 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 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | -------------------------------------------------------------------------------- /cart_humanoid/urdf/hand_meshes/JVRC_finger1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/cart_humanoid/urdf/hand_meshes/JVRC_finger1.stl -------------------------------------------------------------------------------- /cart_humanoid/urdf/hand_meshes/JVRC_finger2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/cart_humanoid/urdf/hand_meshes/JVRC_finger2.stl -------------------------------------------------------------------------------- /cart_humanoid/urdf/hand_meshes/JVRC_palm.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/cart_humanoid/urdf/hand_meshes/JVRC_palm.stl -------------------------------------------------------------------------------- /cart_humanoid/urdf/my_mesh.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/cart_humanoid/urdf/my_mesh.stl -------------------------------------------------------------------------------- /cart_humanoid/urdf/my_model.urdf.xacro: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /cart_humanoid/urdf/robot.urdf.xacro: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /cart_humanoid/worlds/model/my_model/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | my_model 4 | 1.0 5 | model.sdf 6 | 7 | sample 8 | sample@jsk.imi.i.u-tokyo.ac.jp 9 | 10 | sample 11 | 12 | -------------------------------------------------------------------------------- /cart_humanoid/worlds/model/my_model/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 1.0 7 | 8 | 0.1 9 | 0.0 10 | 0.0 11 | 0.1 12 | 0.0 13 | 0.1 14 | 15 | 16 | 17 | -0.1 0.04 0 1.5708 0 0 18 | 19 | 20 | model://my_model/my_mesh.stl 21 | 0.001 0.001 0.001 22 | 23 | 24 | 25 | 26 | -0.1 0.04 0 1.5708 0 0 27 | 28 | 29 | model://my_model/my_mesh.stl 30 | 0.001 0.001 0.001 31 | 32 | 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /cart_humanoid/worlds/model/my_model/my_mesh.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/cart_humanoid/worlds/model/my_model/my_mesh.stl -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package daisya_euslisp_tutorials 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 6.1.0 (2019-11-11) 6 | ------------------ 7 | 8 | 6.0.0 (2019-06-16) 9 | ------------------ 10 | 11 | 5.0.0 (2018-09-14) 12 | ------------------ 13 | 14 | 4.0.1 (2017-11-20) 15 | ------------------ 16 | 17 | 4.0.0 (2017-09-24) 18 | ------------------ 19 | 20 | 3.2.2 (2016-12-01) 21 | ------------------ 22 | 23 | 3.2.1 (2016-11-30) 24 | ------------------ 25 | 26 | 3.2.0 (2016-11-29) 27 | ------------------ 28 | 29 | 3.1.0 (2016-11-07) 30 | ------------------ 31 | 32 | 3.0.1 (2016-11-06) 33 | ------------------ 34 | 35 | 3.0.0 (2016-10-16) 36 | ------------------ 37 | * add :arm-end-coords method, this will solve error, cannot find method :arm-end-coords in (apply #'send self :limb :arm args) 38 | * Contributors: Kei Okada 39 | 40 | 2.1.5 (2015-11-25) 41 | ------------------ 42 | 43 | 2.1.4 (2015-11-24) 44 | ------------------ 45 | 46 | 2.1.3 (2015-11-19) 47 | ------------------ 48 | 49 | 2.1.2 (2015-11-12) 50 | ------------------ 51 | 52 | 2.1.1 (2015-11-11) 53 | ------------------ 54 | 55 | 2.1.0 (2015-11-11) 56 | ------------------ 57 | 58 | 2.0.0 (2015-11-10) 59 | ------------------ 60 | * daisya_euslisp_tutorials: add depends to dxl_armed_turtlebot 61 | * {daisya_euslisp_tutorials,turtleboteus}/{package.xml,CMakeLists.txt}: add rostest to {build,run}_depend 62 | * Contributors: Kei Okada 63 | 64 | 1.0.3 (2015-11-09) 65 | ------------------ 66 | 67 | 1.0.2 (2014-12-01) 68 | ------------------ 69 | 70 | 1.0.1 (2014-11-27) 71 | ------------------ 72 | * test-subsumption should run more than 100, since the sensor data changes at i = 100 73 | * Contributors: Kei Okada 74 | 75 | 1.0.0 (2014-11-11) 76 | ------------------ 77 | * Fix final ik for reaching 78 | * Add find_package for rostest 79 | * Enable to catkin_make test for several examples 80 | * remove rosbuild code 81 | * Move robot-programming enshu packages from source forge repository 82 | * Contributors: Kei Okada, Shunichi Nozawa 83 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(daisya_euslisp_tutorials) 3 | 4 | find_package(catkin REQUIRED COMPONENTS rostest) 5 | 6 | catkin_package() 7 | 8 | add_rostest(test/test-psolve.test) 9 | add_rostest(test/test-maze.test) 10 | add_rostest(test/test-daisya.test) 11 | add_rostest(test/test-daisya-ik.test) 12 | add_rostest(test/test-daisya-maze.test) 13 | add_rostest(test/test-subsumption.test) 14 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/daisya-hanoi.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/daisya_euslisp_tutorials/euslisp/daisya-hanoi.l -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/daisya-ik.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/daisya_euslisp_tutorials/euslisp/daisya-ik.l -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/daisya-maze-2011.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/daisya_euslisp_tutorials/euslisp/daisya-maze-2011.l -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/daisya-maze.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/daisya_euslisp_tutorials/euslisp/daisya-maze.l -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/daisya-qlearn.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/daisya_euslisp_tutorials/euslisp/daisya-qlearn.l -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/daisya-subsumption.l: -------------------------------------------------------------------------------- 1 | (load "daisya-maze.l") 2 | (load "subsumption.l") 3 | 4 | ;; 5 | (defun test-subsumption nil 6 | (let (i) 7 | (setq t1 (instance subsumption-task :init :name "Wander" :priority 10 8 | :check-func #'(lambda (v) t) 9 | :action-func #'(lambda () 10 | (print "wander") 11 | (unix:usleep (* 100 1000))))) 12 | (setq t2 (instance subsumption-task :init :name "RightBumper" :priority 90 13 | :check-func #'(lambda (v) (> (elt v 0) 0)) 14 | :action-func #'(lambda () 15 | (print "right") 16 | (unix:sleep 2) 17 | (print "right done") 18 | ))) 19 | (setq t3 (instance subsumption-task :init :name "LeftBumper" :priority 100 20 | :check-func #'(lambda (v) (< (elt v 0) 0)) 21 | :action-func #'(lambda () 22 | (print "left") 23 | (unix:sleep 3) 24 | (print "left done") 25 | ))) 26 | (setq s (instance subsumption :init (list t1 t2 t3) 27 | :sensor-vector #f(0 0 0 0 0 0) 28 | :debug nil)) 29 | (send s :start-clients) 30 | (setq i 0) 31 | (do-until-key 32 | (cond 33 | ((> i 100) (setq sensor-vector #f( 0 0 0))) 34 | ((> i 50) (setq sensor-vector #f( 10 0 0))) 35 | ((> i 25) (setq sensor-vector #f(-10 0 0))) 36 | (t (setq sensor-vector #f( 0 0 0)))) 37 | (send s :sensor-vector sensor-vector) 38 | (print (list i sensor-vector)) 39 | (unix:usleep (* 100 1000)) 40 | (incf i) 41 | ) 42 | (send s :stop-clients) 43 | )) 44 | 45 | (defun daisya-subsumption nil 46 | (let (i) 47 | (setq t1 (instance subsumption-task :init :name "Wander" :priority 10 48 | :check-func #'(lambda (v) t) 49 | :action-func #'(lambda () 50 | (print "wander") 51 | (send *daisya* :daisya-velocity-vector 52 | (float-vector 10 0 0))))) 53 | (setq t2 (instance subsumption-task :init :name "RightBumper" :priority 80 54 | :check-func #'(lambda (v) (< (elt v 0) 150)) 55 | :action-func #'(lambda () 56 | (print "right") 57 | (send *daisya* :daisya-velocity-vector 58 | (float-vector 10 0 5)) 59 | ))) 60 | (setq t3 (instance subsumption-task :init :name "LeftBumper" :priority 90 61 | :check-func #'(lambda (v) (< (elt v 2) 100)) 62 | :action-func #'(lambda () 63 | (print "left") 64 | (send *daisya* :daisya-velocity-vector 65 | (float-vector 10 0 -15)) 66 | ))) 67 | (setq t4 (instance subsumption-task :init :name "FrontBumper" :priority 100 68 | :check-func #'(lambda (v) (< (elt v 1) 150)) 69 | :action-func #'(lambda () 70 | (print "front") 71 | (send *daisya* :daisya-velocity-vector 72 | (float-vector -10 0 -5)) 73 | ))) 74 | (setq s (instance subsumption :init (list t1 t2 t3 t4) 75 | :sensor-vector #f(0 0 0 0 0 0) 76 | :debug nil)) 77 | (send s :start-clients) 78 | (setq i 0) 79 | (do-until-key 80 | (send *daisya* :simulate (send *maze* :bodies)) 81 | (setq sensor-vector (send *daisya* :psd-vector)) 82 | (send s :sensor-vector sensor-vector) 83 | (print (list i sensor-vector)) 84 | (unix:usleep (* 100 1000)) 85 | (draw-viewers) 86 | (incf i) 87 | ) 88 | (send s :stop-clients) 89 | )) 90 | 91 | (warn ";; type (test-subsumption) for test~%") 92 | (warn ";; type 93 | ;; (maze-init) 94 | ;; (daisya-subsumption) 95 | ;; for daisya subsumption simulation") 96 | 97 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/daisya.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/daisya_euslisp_tutorials/euslisp/daisya.l -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/gsearch.l: -------------------------------------------------------------------------------- 1 | ;;; 2 | ;;; search program 2009.10.25 M.Inaba 3 | ;;; 4 | ;;; 5 | ;;; graph 6 | ;;; 7 | (defun node= (a b) (equal a b)) 8 | 9 | (defun a-list (v g &key (test #'node=)) 10 | (cond ((null g) nil) 11 | ((set-member v (car g) :test test) 12 | (set+ (set-remove v (car g) :test test) 13 | (a-list v (cdr g) :test test) 14 | :test test)) 15 | (t (a-list v (cdr g) :test test)))) 16 | 17 | (defun new-nodes (path graph &key (test #'node=)) 18 | (p-extend path 19 | (a-list (first path) graph :test test) 20 | :test test)) 21 | 22 | (defun p-extend (path nodes &key (test #'node=)) 23 | (cond ((null nodes) nil) 24 | ((set-member (car nodes) path :test test) 25 | (p-extend path (cdr nodes) :test test)) 26 | (t (cons (car nodes) 27 | (p-extend path (cdr nodes) :test test))))) 28 | 29 | (defun new-path-list (path graph &key (test #'node=)) 30 | (mapcar #'(lambda (n) (cons n path)) 31 | (new-nodes path graph :test test))) 32 | ;;; 33 | ;;; 34 | ;;; 35 | (defun depth-first (s f g &optional (p (list s)) (pq (list p))) 36 | (cond ((null pq) nil) 37 | ((node= f (first p)) (reverse p)) 38 | (t (depth-first 39 | s f g 40 | (car pq) 41 | (append (new-path-list p g) (rest pq)))))) 42 | 43 | ;;; 44 | (defun depth-first (s f g &optional (p (list s)) (pq (list p))) 45 | (do () 46 | ((or (null pq) (node= f (first p))) 47 | (if (null pq) nil (reverse p))) 48 | (setq p (car pq)) 49 | (setq pq (append (new-path-list p g) (rest pq)))) 50 | ) 51 | ;;; 52 | ;;; 53 | (defun breadth-first (s f g &optional (p (list s)) (pq (list p))) 54 | (cond ((null pq) nil) 55 | ((node= f (first p)) (reverse p)) 56 | (t (breadth-first 57 | s f g 58 | (car pq) 59 | (append (rest pq) (new-path-list p g)))))) 60 | ;;; 61 | ;;; 62 | (setq *g* '((s a) (s d) (a b) (a d) (b c) (b e) (d e) (e f))) 63 | ;;; 64 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/irteusext.l: -------------------------------------------------------------------------------- 1 | (defun hvs2rgb (h &optional i s ret) 2 | (when (or (listp h) (vectorp h)) 3 | (setq ret s) 4 | (setq s (elt h 2) i (elt h 1) h (elt h 0))) 5 | (if (>= h 360) (setq h (mod (round h) 360))) 6 | (if (< h 0) (setq h (- 360 (mod (round (- h)) 360)))) 7 | (setq s (* 255 s) i (* 255 i)) 8 | (let* ((hi (floor (/ h 60.0))) 9 | (f (- (/ h 60.0) hi)) 10 | (p1 (* i (- 1.0 (/ s 255.0)))) 11 | (p2 (* i (- 1.0 (* f (/ s 255.0))))) 12 | (p3 (* i (- 1.0 (* (- 1 f) (/ s 255.0))))) 13 | r g b) 14 | (case hi 15 | ((0 6) (setq r i g p3 b p1)) 16 | (1 (setq r p2 g i b p1)) 17 | (2 (setq r p1 g i b p3)) 18 | (3 (setq r p1 g p2 b i)) 19 | (4 (setq r p3 g p1 b i)) 20 | (5 (setq r i g p1 b p2)) 21 | (t (warn "hri2rgb error"))) 22 | (cond 23 | (ret 24 | (setf (elt ret 0) r) 25 | (setf (elt ret 1) g) 26 | (setf (elt ret 2) b) 27 | ret) 28 | (t (list r g b))))) 29 | 30 | 31 | (defclass psd-model 32 | :super sensor-model 33 | :slots (max targetp detectp)) 34 | (defmethod psd-model 35 | (:init 36 | ;;だいたい最大15cmくらいしかとってこれない 37 | (shape &rest args &key ((:max m) 150)) 38 | (if m (setq max m)) 39 | ;;距離sensorはdefault(みつからないとき)は0がかえってくる仕様 40 | (setq data 0) 41 | (setq detectp nil) 42 | (send-super* :init shape args) 43 | ) 44 | ;; 45 | ;;objsには距離をもとめたい対象となるbodiesが入る 46 | (:simulate 47 | (objs) 48 | (let* (r (mind max) 49 | (p (send self :worldpos)) 50 | ;;z方向（sensorの視線方向のベクトルを保存) 51 | (v (matrix-column (send self :worldrot) 2))) 52 | (if (atom objs) (setq objs (list objs))) 53 | ;; 54 | (setq detectp nil) 55 | (dolist (obj objs) 56 | (dolist (f (send obj :faces)) 57 | ;;点を通りベクトル方向の線分と面の交点を求める 58 | (setq r (send f :intersect-point-vector (send self :worldpos) v)) 59 | (when (eq (car r) :inside) ;;もし交点が面の中にあったら（距離sensorがぶつかるということ) 60 | (when (< (distance p (cadr r)) mind) 61 | (setq targetp (cadr r) mind (distance p targetp)) 62 | (setq detectp t) 63 | )))) 64 | (cond 65 | (detectp 66 | (setq data mind)) 67 | ;;maxをこえている場合や、みつからない場合は0とする 68 | (t 69 | (setq data 0))) 70 | )) 71 | (:draw (vwer) (send self :draw-sensor vwer)) 72 | ;; 73 | ;; 74 | (:draw-sensor 75 | (vwer) 76 | (let ((pwidth (send vwer :viewsurface :line-width)) 77 | (pcolor (send vwer :viewsurface :color)) 78 | (v (float-vector 0 0 0))) 79 | (when detectp 80 | (setq v (normalize-vector (hvs2rgb (/ (* data -120.0) max) 0.9 0.9 v))) 81 | (send vwer :viewsurface :line-width 3) 82 | (send vwer :viewsurface :color v) 83 | (send vwer :viewsurface :3d-line (send self :worldpos) targetp) 84 | (send vwer :viewsurface :line-width pwidth) 85 | (send vwer :viewsurface :color pcolor)) 86 | (gl::draw-glbody vwer self) 87 | )) 88 | ) 89 | 90 | (defmethod camera-model 91 | (:simulate 92 | (model) 93 | (let (bs p w ret) 94 | (setq bs 95 | (mapcan 96 | #'(lambda (b) 97 | (if (and (vectorp (get b :face-color)) 98 | (eps-v= #f(1 0 0) (get b :face-color) 0.1)) 99 | (list b))) model)) 100 | (dolist (b bs) 101 | (setq w (elt (send self :viewing :view (send b :worldpos)) 2)) 102 | (setq p (send self :screen-point (send b :worldpos))) 103 | (if (and (> w 0) 104 | (< 0 (elt p 0) pwidth) (< 0 (elt p 1) pheight) 105 | (< (distance (send self :worldpos) (send b :worldpos)) 106 | (send self :yon))) 107 | (push (list :centroid p) ret))) 108 | (setq data ret) 109 | ret)) 110 | ) 111 | 112 | 113 | 114 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/maze.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/daisya_euslisp_tutorials/euslisp/maze.l -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/robot-server-simulator.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/daisya_euslisp_tutorials/euslisp/robot-server-simulator.l -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/sample-robot-server.bak.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/daisya_euslisp_tutorials/euslisp/sample-robot-server.bak.l -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/sample-sim-real.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/daisya_euslisp_tutorials/euslisp/sample-sim-real.l -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/sample-sim.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/daisya_euslisp_tutorials/euslisp/sample-sim.l -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/sets.l: -------------------------------------------------------------------------------- 1 | ;;; 2 | ;;; set operation 3 | ;;; 4 | (defun set-member (e set &key (test #'eq)) 5 | (cond 6 | ((null set) nil) 7 | ((funcall test e (car set)) t) 8 | (t (set-member e (cdr set) :test test)))) 9 | 10 | (defun set-member (e set &key (test #'eq)) 11 | (do () 12 | ((or (null set) (funcall test e (car set))) 13 | (if (null set) nil t)) 14 | (setq set (cdr set)))) 15 | 16 | (defun set-adjoin (a s &key (test #'eq)) ;; adjoin 17 | (if (set-member a s :test test) s (cons a s))) 18 | 19 | (defun set-remove (e set &key (test #'eq)) 20 | (cond 21 | ((null set) nil) 22 | ((funcall test e (car set)) 23 | (set-remove e (cdr set) :test test)) 24 | (t (cons (car set) 25 | (set-remove e (cdr set) :test test))))) 26 | 27 | (defun set-remove (e set &key (test #'eq)) 28 | (do ((r nil)) 29 | ((null set) r) 30 | (if (null (funcall test e (car set))) 31 | (setq r (cons (car set) r))) 32 | (setq set (cdr set)))) 33 | 34 | (defun set+ (a b &key (test #'eq)) 35 | (cond ((null a) b) 36 | ((set-member (car a) b :test test) 37 | (set+ (cdr a) b :test test)) 38 | (t (cons (car a) (set+ (cdr a) b :test test))))) 39 | 40 | (defun set+ (a b &key (test #'eq)) 41 | (do ((r b)) 42 | ((null a) r) 43 | (if (not (set-member (car a) b :test test)) 44 | (setq r (cons (car a) r))) 45 | (setq a (cdr a)))) 46 | 47 | (defun set- (a b &key (test #'eq)) 48 | (cond 49 | ((null a) nil) 50 | ((set-member (car a) b :test test) 51 | (set- (cdr a) b :test test)) 52 | (t (cons (car a) (set- (cdr a) b :test test))))) 53 | 54 | (defun set* (a b &key (test #'eq)) 55 | (cond 56 | ((null a) nil) 57 | ((set-member (car a) b :test test) 58 | (cons (car a) (set* (cdr a) b :test test))) 59 | (t (set* (cdr a) b :test test)))) 60 | 61 | (defun set^ (a b &key (test #'eq)) 62 | (cond 63 | ((null a) b) 64 | ((set-member (car a) b :test test) 65 | (set^ (cdr a) (remove (car a) b :test test) :test test)) 66 | (t (cons (car a) (set^ (cdr a) b :test test))))) 67 | 68 | (defun set< (a b &key (test #'eq)) 69 | (cond 70 | ((null a) t) 71 | ((set-member (car a) b :test test) 72 | (set< (cdr a) b :test test)) 73 | (t nil))) 74 | 75 | (defun set= (a b &key (test #'eq)) 76 | (null (set^ a b :test test))) 77 | 78 | ;;; 79 | (defun eq-classes (x &key (test #'eq)) 80 | (cond 81 | ((null x) nil) 82 | (t (eq-classes-aux 83 | (car x) 84 | (eq-classes (cdr x) :test test) 85 | :test test)))) 86 | 87 | (defun eq-classes-aux (a b &key (test #'eq)) 88 | (cond 89 | ((null b) (list a)) 90 | ((set* a (car b) :test test) 91 | (eq-classes-aux 92 | (set+ a (car b) :test test) 93 | (cdr b) 94 | :test test)) 95 | (t (cons (car b) 96 | (eq-classes-aux 97 | a (cdr b) 98 | :test test))))) 99 | #| 100 | > (eq-classes '((a b) (c d) (a c))) 101 | ((a b c d)) 102 | > (eq-classes '((s a) (s d) (a b) (a d) 103 | (b c) (b e) (d e) (e f))) 104 | ((s a d b c e f)) 105 | > (eq-classes '((1 2) (2 3) (4 5) (6 5))) 106 | ((4 6 5) (1 2 3)) 107 | > (eq-classes 108 | '(((1) (2)) ((2) (3)) ((4) (5)) ((6) (5)))) 109 | (((6) (5)) ((4) (5)) ((2) (3)) ((1) (2))) 110 | > (eq-classes 111 | '(((1) (2)) ((2) (3)) ((4) (5)) ((6) (5))) 112 | :test #'equal) 113 | (((4) (6) (5)) ((1) (2) (3))) 114 | |# -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/euslisp/virtual_interpolator.l: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/daisya_euslisp_tutorials/euslisp/virtual_interpolator.l -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b daisya_euslisp_tutorials 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | daisya_euslisp_tutorials 3 | 7.0.0 4 | Daisya enshu 5 | BSD 6 | Shunichi Nozawa 7 | Shunichi Nozawa 8 | 9 | catkin 10 | 11 | roseus 12 | rostest 13 | dxl_armed_turtlebot 14 | 15 | roseus 16 | rostest 17 | dxl_armed_turtlebot 18 | 19 | 20 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-daisya-ik.l: -------------------------------------------------------------------------------- 1 | #+:ros (require :unittest "lib/llib/unittest.l") 2 | 3 | (unless *unit-test* 4 | (init-unit-test)) 5 | 6 | (load "package://daisya_euslisp_tutorials/euslisp/daisya-ik.l") 7 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l") 8 | 9 | ;; overwrite for test 10 | (defun read-line (&rest args) 11 | ;; h:left, j:down, k:up, l:right, f:forward, b:back 12 | (prog1 13 | (case *count-for-test* 14 | (0 "h") 15 | (1 "j") 16 | (2 "k") 17 | (3 "l") 18 | (4 "f") 19 | (5 "b") 20 | (6 "e") 21 | ) 22 | (incf *count-for-test*) 23 | )) 24 | 25 | (deftest test-daisya-ik-normal 26 | (setq *count-for-test* 0) 27 | (assert (ik-demo0 :use-base nil))) 28 | 29 | (deftest test-daisya-ik-base 30 | (setq *count-for-test* 0) 31 | (assert (ik-demo0 :use-base t))) 32 | 33 | (deftest test-dxl-armed-turtlebot-ik-normal 34 | (setq *count-for-test* 0) 35 | (assert (ik-demo0 :use-base nil :robot (instance dxl-armed-turtlebot-robot :init)))) 36 | 37 | (deftest test-dxl-armed-turtlebot-ik-base 38 | (setq *count-for-test* 0) 39 | (assert (ik-demo0 :use-base t :robot (instance dxl-armed-turtlebot-robot :init)))) 40 | 41 | (run-all-tests) 42 | (exit 0) -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-daisya-ik.test: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-daisya-maze.l: -------------------------------------------------------------------------------- 1 | #+:ros (require :unittest "lib/llib/unittest.l") 2 | 3 | (unless *unit-test* 4 | (init-unit-test)) 5 | 6 | (defun init-test () 7 | ;; daisya 8 | (load "package://daisya_euslisp_tutorials/euslisp/daisya.l") 9 | (setq *daisya* (instance daisyaclass :init)) 10 | (objects (list *daisya*)) 11 | ;; dxl-armed-turtlebot-robot 12 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l") 13 | (setq *dxl-armed-turtlebot* (instance dxl-armed-turtlebot-robot :init)) 14 | (objects (list *dxl-armed-turtlebot*)) 15 | ) 16 | (init-test) 17 | 18 | (deftest test-maze-init 19 | (assert (load "package://daisya_euslisp_tutorials/euslisp/daisya-maze.l")) 20 | (assert (maze-init)) 21 | (assert (maze-demo 1 25 :extender #'depth-extender)) 22 | (assert (maze-demo 1 25 :extender #'breadth-extender)) 23 | (assert (maze-demo 1 25 :extender #'hill-climb-extender)) 24 | (assert (maze-demo 1 25 :extender #'best-first-extender)) 25 | (assert (maze-demo 1 25 :extender #'a*-extender)) 26 | (assert (maze-init :robot *dxl-armed-turtlebot*)) 27 | (assert (maze-demo 1 25 :extender #'depth-extender :robot *dxl-armed-turtlebot*)) 28 | (assert (maze-demo 1 25 :extender #'breadth-extender :robot *dxl-armed-turtlebot*)) 29 | (assert (maze-demo 1 25 :extender #'hill-climb-extender :robot *dxl-armed-turtlebot*)) 30 | (assert (maze-demo 1 25 :extender #'best-first-extender :robot *dxl-armed-turtlebot*)) 31 | (assert (maze-demo 1 25 :extender #'a*-extender :robot *dxl-armed-turtlebot*)) 32 | ) 33 | 34 | (run-all-tests) 35 | (exit 0) -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-daisya-maze.test: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-daisya.l: -------------------------------------------------------------------------------- 1 | #+:ros (require :unittest "lib/llib/unittest.l") 2 | 3 | (unless *unit-test* 4 | (init-unit-test)) 5 | 6 | (deftest test-daisya-init 7 | (assert (load "package://daisya_euslisp_tutorials/euslisp/daisya.l")) 8 | (assert (setq *daisya* (instance daisyaclass :init))) 9 | (assert (objects (list *daisya*))) 10 | (assert (send *daisya* :angle-vector #f(0.0 0.0 0.0 0.0 30.0))) ;; gripper opening 11 | ) 12 | 13 | (deftest test-dxl-armed-turtlebot-init 14 | (assert (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l")) 15 | (assert (setq *dxl-armed-turtlebot* (instance dxl-armed-turtlebot-robot :init))) 16 | (assert (objects (list *dxl-armed-turtlebot*))) 17 | (assert (send *dxl-armed-turtlebot* :angle-vector #f(0.0 0.0 0.0 0.0 0.0 0.0 30.0))) ;; gripper opening 18 | ) 19 | 20 | (run-all-tests) 21 | (exit 0) -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-daisya.test: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-maze.l: -------------------------------------------------------------------------------- 1 | #+:ros (require :unittest "lib/llib/unittest.l") 2 | 3 | (unless *unit-test* 4 | (init-unit-test)) 5 | 6 | (defun init-test () 7 | ;; daisya 8 | (load "package://daisya_euslisp_tutorials/euslisp/daisya.l") 9 | (setq *daisya* (instance daisyaclass :init)) 10 | (objects (list *daisya*)) 11 | ;; dxl-armed-turtlebot-robot 12 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l") 13 | (setq *dxl-armed-turtlebot* (instance dxl-armed-turtlebot-robot :init)) 14 | (objects (list *dxl-armed-turtlebot*)) 15 | ) 16 | (init-test) 17 | 18 | (deftest test-maze-init 19 | (assert (load "package://daisya_euslisp_tutorials/euslisp/maze.l")) 20 | (assert (setq *maze* (make-maze *m*))) 21 | (assert (objects (list *maze* *daisya*))) 22 | (assert (send *daisya* :locate (generate-coords-from-nodenum 1) :world)) 23 | (assert (objects (list *maze* *dxl-armed-turtlebot*))) 24 | (assert (send *dxl-armed-turtlebot* :locate (generate-coords-from-nodenum 1) :world)) 25 | ) 26 | 27 | (run-all-tests) 28 | (exit 0) -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-maze.test: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-psolve.l: -------------------------------------------------------------------------------- 1 | #+:ros (require :unittest "lib/llib/unittest.l") 2 | 3 | (unless *unit-test* 4 | (init-unit-test)) 5 | 6 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l") 7 | (load "package://daisya_euslisp_tutorials/euslisp/psolve.l") 8 | 9 | (deftest test-mase-*M* 10 | (assert *m*)) 11 | 12 | (deftest test-mase-search-normal 13 | (assert (maze-search 7 25 *m*))) 14 | 15 | (deftest test-mase-search-breadth-extender 16 | (assert (maze-search 7 21 *m* :extender #'breadth-extender))) 17 | 18 | (deftest test-mase-search-a*-extender 19 | (assert (maze-search 7 21 *m* :extender #'a*-extender))) 20 | 21 | (run-all-tests) 22 | (exit 0) -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-psolve.test: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-sample-robot-server.bak.l: -------------------------------------------------------------------------------- 1 | #+:ros (require :unittest "lib/llib/unittest.l") 2 | 3 | (unless *unit-test* 4 | (init-unit-test)) 5 | 6 | (deftest test-sample-robot-server 7 | (assert (load "package://daisya_euslisp_tutorials/euslisp/sample-robot-server.bak.l")) 8 | (assert (progn (init-server) t)) 9 | (assert (connect-robotsim-server)) 10 | (assert (progn (send *rs-sim* :daisya-velocity-vector #f(100 0 0)) t)) 11 | (assert (progn (send *rs-sim* :daisya-velocity-vector #f(0 0 0)) t)) 12 | (assert (progn (send *rs-sim* :arm-poweron-vector #f(1 1 1 1 0)) t)) 13 | (assert 14 | (progn 15 | (send *rs-sim* :arm-angle-vector #f(80 20 70 0 0) 1000) 16 | (unix:sleep 1) 17 | t)) 18 | ) 19 | 20 | (deftest test-sample-robot-server-maze 21 | (assert (load "package://daisya_euslisp_tutorials/euslisp/sample-robot-server.bak.l")) 22 | (assert (progn (init-server 'maze) t)) 23 | (assert (connect-robotsim-server)) 24 | (assert (progn (send *rs-sim* :daisya-velocity-vector #f(100 0 0)) t)) 25 | (assert (progn (send *rs-sim* :daisya-velocity-vector #f(0 0 0)) t)) 26 | (assert (progn (send *rs-sim* :arm-poweron-vector #f(1 1 1 1 0)) t)) 27 | (assert 28 | (progn 29 | (send *rs-sim* :arm-angle-vector #f(80 20 70 0 0) 1000) 30 | (unix:sleep 1) 31 | t)) 32 | ) 33 | 34 | (run-all-tests) 35 | (exit 0) -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-sample-robot-server.bak.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-sample-robot-server.l: -------------------------------------------------------------------------------- 1 | #+:ros (require :unittest "lib/llib/unittest.l") 2 | 3 | (unless *unit-test* 4 | (init-unit-test)) 5 | 6 | (defmacro read-char-case 7 | (&rest args) 8 | (prog1 9 | (case *count* 10 | (0 90) 11 | (1 -90) 12 | (2 90) 13 | (3 -90) 14 | (4 nil) 15 | ) 16 | (incf *count*))) 17 | 18 | (deftest test-sample-robot-server-demo0 19 | (assert (load "package://daisya_euslisp_tutorials/euslisp/sample-robot-server.l")) 20 | (assert (progn (demo0) t)) 21 | ) 22 | 23 | (deftest test-sample-robot-server-demo1 24 | (setq *count* 0) 25 | (assert (load "package://daisya_euslisp_tutorials/euslisp/sample-robot-server.l")) 26 | (assert (progn (demo1) t)) 27 | ) 28 | 29 | (run-all-tests) 30 | (exit 0) -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-sample-robot-server.launch: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-subsumption.l: -------------------------------------------------------------------------------- 1 | #+:ros (require :unittest "lib/llib/unittest.l") 2 | 3 | (unless *unit-test* 4 | (init-unit-test)) 5 | 6 | (deftest test-subsumption 7 | (assert (load "package://daisya_euslisp_tutorials/euslisp/subsumption.l")) 8 | (assert (test-subsumption :exit-count 100))) 9 | 10 | (run-all-tests) 11 | (exit 0) -------------------------------------------------------------------------------- /daisya_euslisp_tutorials/test/test-subsumption.test: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dxl_armed_turtlebot) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roseus) # add roseus to gen messages 5 | 6 | catkin_package() 7 | 8 | if(CATKIN_ENABLE_TESTING) 9 | find_package(catkin REQUIRED COMPONENTS rostest roslaunch) 10 | # roslacunch check 11 | file(GLOB LAUNCH_FILES launch/*.launch) 12 | foreach(LAUNCH_FILE ${LAUNCH_FILES}) 13 | if("${LAUNCH_FILE}" STREQUAL "${CMAKE_CURRENT_SOURCE_DIR}/launch/dxl_armed_turtlebot_bringup.launch") 14 | find_package(freenect_launch QUIET) 15 | if(NOT freenect_launch_FOUND) 16 | continue() 17 | endif() 18 | endif() 19 | if("${LAUNCH_FILE}" STREQUAL "${CMAKE_CURRENT_SOURCE_DIR}/launch/checkerboard_detector_5x4.launch" 20 | OR "${LAUNCH_FILE}" STREQUAL "${CMAKE_CURRENT_SOURCE_DIR}/launch/checkerboard_detector_7x4.launch" 21 | OR "${LAUNCH_FILE}" STREQUAL "${CMAKE_CURRENT_SOURCE_DIR}/launch/checkerboard_detector.launch") 22 | # to avoid error: "ROSLaunch config error: unrecognized tag sphinxdoc" 23 | # https://github.com/jsk-ros-pkg/jsk_roseus/blob/master/roseus_tutorials/launch/checkerboard-detector.launch#L35 24 | continue() 25 | endif() 26 | roslaunch_add_file_check(${LAUNCH_FILE}) 27 | endforeach() 28 | # test 29 | find_package(pr2eus QUIET) 30 | if(pr2eus_FOUND) 31 | add_rostest(test/test-dxl-armed-turtlebot.test) 32 | endif() 33 | endif() 34 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/README.md: -------------------------------------------------------------------------------- 1 | ## Smaples in Gazebo 2 | 3 | まずは、下記のコマンドを実行し、gazeboシステム及び仮想ロボットを立ち上げます 4 | ``` 5 | $ roslaunch dxl_armed_turtlebot dxl_armed_turtlebot_gazebo.launch 6 | ``` 7 | 8 | その場合、以下のような画面になりと思います。 9 | 10 | ![gazebo_turtlebot](images/gazebo_turtlebot2.png) 11 | 12 | #### 注意: 13 | gazeboのgui (右図) を立ち上げると、PCの処理負荷が一気に上がるので、通常は`gui:=false`というオプションをつけて、rvizのみ表示させることを推奨します。 14 | その場合、コマンドは以下のようになります。 15 | 16 | ``` 17 | $ roslaunch dxl_armed_turtlebot dxl_armed_turtlebot_gazebo.launch gui:=false 18 | ``` 19 | 20 | ### 1. Grasping test in gazebo via roseus 21 | 22 | 以下のコマンドを実行し、roseusから目の前にある物体を掴んでみましょう。 23 | 24 | ``` 25 | $ roscd dxl_armed_turtlebot/euslisp 26 | $ roseus gazebo-grasp-test.l 27 | ``` 28 | 29 | このサンプルでは、eusのロボットモデルである`*ri*`を駆使して、台車移動、アーム操作、グリッパー操作という基本動作を実行しています。 30 | 成功すると以下のようになります(gazebo guiの表示画面)。 31 | 32 | ![gazebo_turtlebot_grasp](images/gazebo_turtlebot_grasp.png) 33 | 34 | ### 2. Object recognition and approaching via roseus 35 | 前の例では、物体認識を行っておりません。物体認識を踏まえた自律動作は以下のように実行します。 36 | 37 | 1. 物体認識を実行する. これは演習で習ったもので、詳細は演習資料を参考してください。 38 | ``` 39 | $ roslaunch dxl_armed_turtlebot hsi_color_filter.launch DEFAULT_NAMESPACE:=/camera/depth INPUT:=points h_min:=-120 h_max:=-20 s_min:=120 40 | ``` 41 | 2. 認識結果を元に、自律行動を実行する 42 | ``` 43 | $ roscd dxl_armed_turtlebot/euslisp 44 | $ roseus gazebo-grasp-with-recognition.l 45 | ``` 46 | 47 | ### 3. SLAM in Gazebo 48 | gazebo内でも地図作成は、実機と同様、下記のコマンドを実行するだけ: 49 | ``` 50 | $ rosrun slam_karto slam_karto 51 | ``` 52 | 53 | rviz上で、2D地図ができていることが確認できるでしょう。 54 | ロボットの移動はroseusからでもできるが、ここでは、下記のコマンドを紹介します: 55 | ``` 56 | $ roslaunch turtlebot_teleop keyboard_teleop.launch 57 | ``` 58 | 機体を一周旋回させると、以下のようになります。 59 | 60 | ![gazebo_turtlebot_slam](images/gazebo_turtlebot_slam.png) 61 | 62 | ### 4. Detect Checkerboard in Gazebo 63 | gazebo内でも実機と同様にcheckerboardを検出することができる 64 | 65 | まず、checkerboader_detectorを起動しましょう: 66 | ``` 67 | $ roslaunch roseus_tutorials checkerboard-detector.launch rect0_size_x:=0.02 rect0_size_y:=0.02 grid0_size_x:=7 grid0_size_y:=4 translation0:="0 0 0" image:=image_raw group:=/camera/rgb frame_id:=camera_rgb_optical_frame 68 | ``` 69 | 70 | 71 | 次に、上記と同じコマンドを使って、ロボットを旋回させましょう: 72 | ``` 73 | $ roslaunch turtlebot_teleop keyboard_teleop.launch 74 | ``` 75 | 76 | 最後に、rviz上でchecker_boardが見つかれば、下記のコマンドでroseus上でcheckerboardを表示しましょう: 77 | 78 | ``` 79 | $ roscd dxl_armed_turtlebot/euslisp 80 | $ roseus display-checkerboard.l 81 | ``` 82 | 83 | 最終てきには、以下のようになります。 84 | 85 | ![gazebo_checkerboard](images/gazebo_checkerboard.png) 86 | 87 | 88 | 89 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/config/dxl_armed_turtlebot_controllers.yaml: -------------------------------------------------------------------------------- 1 | # Joint State Controllers --------------------------------------- 2 | joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 50 5 | # Trajectory Controllers --------------------------------------- 6 | fullbody_controller: 7 | #type: effort_controllers/JointTrajectoryController 8 | type: velocity_controllers/JointTrajectoryController 9 | joints: 10 | - arm_joint1 11 | - arm_joint2 12 | - arm_joint3 13 | - arm_joint4 14 | - arm_joint5 15 | - arm_joint6 16 | constraints: 17 | goal_time: 0.5 # Override default 18 | stopped_velocity_tolerance: 0.02 # Override default 19 | arm_joint1: 20 | #trajectory: 0.1 # Not enforced if unspecified 21 | goal: 0.02 # Not enforced if unspecified 22 | arm_joint2: 23 | #trajectory: 0.1 # Not enforced if unspecified 24 | goal: 0.02 # Not enforced if unspecified 25 | arm_joint3: 26 | #trajectory: 0.1 # Not enforced if unspecified 27 | goal: 0.02 # Not enforced if unspecified 28 | arm_joint4: 29 | #trajectory: 0.1 # Not enforced if unspecified 30 | goal: 0.02 # Not enforced if unspecified 31 | arm_joint5: 32 | #trajectory: 0.1 # Not enforced if unspecified 33 | goal: 0.02 # Not enforced if unspecified 34 | arm_joint6: 35 | #trajectory: 0.1 # Not enforced if unspecified 36 | goal: 0.02 # Not enforced if unspecified 37 | 38 | gains: # Required because we're controlling an effort interface 39 | arm_joint1: {p: 25.0, i: 0.0, d: 0.0} 40 | arm_joint2: {p: 25.0, i: 0.0, d: 0.0} 41 | arm_joint3: {p: 25.0, i: 0.0, d: 0.0} 42 | arm_joint4: {p: 25.0, i: 0.0, d: 0.0} 43 | arm_joint5: {p: 25.0, i: 0.0, d: 0.0} 44 | arm_joint6: {p: 25.0, i: 0.0, d: 0.0} 45 | state_publish_rate: 100 # Override default 46 | action_monitor_rate: 100 # Override default 47 | stop_trajectory_duration: 0 # Override default 48 | 49 | gripper_controller: 50 | #type: effort_controllers/JointTrajectoryController 51 | type: velocity_controllers/JointTrajectoryController 52 | joints: 53 | - gripper_joint 54 | constraints: 55 | goal_time: 0.5 # Override default 56 | stopped_velocity_tolerance: 0.02 # Override default 57 | gripper_joint: 58 | #trajectory: 0.05 # Not enforced if unspecified 59 | goal: 0.02 # Not enforced if unspecified 60 | 61 | gains: # Required because we're controlling an effort interface 62 | gripper_joint: {p: 10.0, i: 0.0, d: 0.0} 63 | state_publish_rate: 100 # Override default 64 | action_monitor_rate: 100 # Override default 65 | stop_trajectory_duration: 0 # Override default 66 | 67 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/config/map.pgm: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/dxl_armed_turtlebot/config/map.pgm -------------------------------------------------------------------------------- /dxl_armed_turtlebot/config/map.yaml: -------------------------------------------------------------------------------- 1 | image: map.pgm 2 | resolution: 0.050000 3 | origin: [-3.942604, -3.959685, 0.000000] 4 | negate: 0 5 | occupied_thresh: 0.65 6 | free_thresh: 0.196 7 | 8 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/doc/Makefile: -------------------------------------------------------------------------------- 1 | TMPDIR := $(shell mktemp -d) 2 | 3 | all: robot_programming_manual.pdf html 4 | 5 | copy_eus_tex: 6 | mkdir -p ${TMPDIR} 7 | cp robot_programming_manual.tex ${TMPDIR} 8 | cp robot_programming_*.l ${TMPDIR} 9 | 10 | robot_programming_manual.pdf: copy_eus_tex 11 | (cd ${TMPDIR};\ 12 | roseus ../eus/lib/llib/documentation.l "(progn\ 13 | (require :robot-interface \"package://pr2eus/robot-interface.l\") (require \"package://turtleboteus/euslisp/turtlebot-with-sensors-robot.l\") (ros::load-ros-manifest \"kobuki_msgs\") (ros::load-ros-manifest \"dynamixel_msgs\") (ros::load-ros-manifest \"dynamixel_controllers\") (ros::load-ros-manifest \"control_msgs\") (ros::load-ros-manifest \"move_base_msgs\") (ros::load-ros-manifest \"diagnostic_msgs\")(ros::load-ros-manifest \"linux_hardware\")\ 14 | (make-document \"robot_programming_model.l\" \"robot_programming_model-func.tex\")\ 15 | (exit))";\ 16 | roseus ../eus/lib/llib/documentation.l "(progn\ 17 | (require :robot-interface \"package://pr2eus/robot-interface.l\") (require \"package://turtleboteus/euslisp/turtlebot-with-sensors-robot.l\") (ros::load-ros-manifest \"kobuki_msgs\") (ros::load-ros-manifest \"dynamixel_msgs\") (ros::load-ros-manifest \"dynamixel_controllers\") (ros::load-ros-manifest \"control_msgs\") (ros::load-ros-manifest \"move_base_msgs\") (ros::load-ros-manifest \"diagnostic_msgs\")(ros::load-ros-manifest \"linux_hardware\") (load \"robot_programming_model.l\")\ 18 | (make-document \"robot_programming_ri.l\" \"robot_programming_ri-func.tex\")\ 19 | (exit))") 20 | nkf --in-place -e ${TMPDIR}/*.tex; 21 | (cd ${TMPDIR}; platex robot_programming_manual; dvipdfmx robot_programming_manual.dvi) 22 | 23 | open: 24 | gnome-open robot_programming_manual.pdf 25 | gnome-open html/index.html 26 | 27 | html: robot_programming_manual.pdf 28 | nkf --in-place -u ${TMPDIR}/*.tex 29 | mkdir ${TMPDIR}/html 30 | sed -i 's@^\(\\\usepackage.*{hyperref}\)@%\1@' ${TMPDIR}/robot_programming_manual.tex 31 | sed -i 's@\\\documentclass\[\]{jarticle}@\\\documentclass\[\]{article}@' ${TMPDIR}/robot_programming_manual.tex 32 | sed -i 's@^\\\ifnum 42146@%\\\ifnum 42146@' ${TMPDIR}/robot_programming_manual.tex 33 | (cd ${TMPDIR}; charset="UTF-8" latex2html -dir ${TMPDIR}/html -local_icons -auto_prefix -iso_language JP robot_programming_manual) 34 | (cd ${TMPDIR}/html; sed -i 's@@\n@' *.html index.html) 35 | sed -i 's@%\(\\\usepackage.*{hyperref}\)@\1@' ${TMPDIR}/robot_programming_manual.tex 36 | sed -i 's@\\\documentclass\[\]{article}@\\\documentclass\[\]{jarticle}@' ${TMPDIR}/robot_programming_manual.tex 37 | sed -i 's@^%\\\ifnum 42146@\\\ifnum 42146@' ${TMPDIR}/robot_programming_manual.tex 38 | cp -r ${TMPDIR}/html ./ 39 | cp ${TMPDIR}/robot_programming_manual.pdf ./html/ 40 | rm -fr ${TMPDIR} 41 | 42 | clean: 43 | rm -f *.log *.out *.toc *.dvi *.idx *.aux *.pdf robot_programming_model-func.tex robot_programming_ri-func.tex 44 | rm -fr html ${TMPDIR} 45 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/doc/robot_programming_manual.tex: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/dxl_armed_turtlebot/doc/robot_programming_manual.tex -------------------------------------------------------------------------------- /dxl_armed_turtlebot/doc/robot_programming_model.l: -------------------------------------------------------------------------------- 1 | (require :turtlebot-with-sensors-robot "package://turtleboteus/euslisp/turtlebot-with-sensors-robot.l") 2 | (require :dxl-7dof-arm-robot "package://dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l") 3 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l") 4 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/doc/robot_programming_ri.l: -------------------------------------------------------------------------------- 1 | (load "package://turtleboteus/euslisp/turtlebot-interface.l") 2 | (load "package://dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface.l") 3 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") 4 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/euslisp/arm-move-sample.l: -------------------------------------------------------------------------------- 1 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") 2 | 3 | (warn ";; (dxl-arm-sample) ;; for dxl arm sample program~%") 4 | (warn ";; (dxl-arm-check) ;; for dxl arm direction check~%") 5 | 6 | (defvar *arm-dof* 7) 7 | 8 | (defun dxl-arm-sample () 9 | (warn ";; press enter to stop program~%~%") 10 | (do-until-key 11 | (warn "set model's (*dxl-armed-turtlebot*) joint angle to :init-pose~%~%") 12 | (send *dxl-armed-turtlebot* :init-pose) ;; 関節角度を :init-pose にセット 13 | (send *irtviewer* :draw-objects) ;; 描画 14 | (unix::usleep (* 2000 1000)) 15 | ;; 16 | (warn "move real robot (*ri*) with 4000 [ms]~%~%") 17 | (send *ri* :angle-vector (send *dxl-armed-turtlebot* :angle-vector) 4000) ;;4000[ms] で実行 18 | (send *ri* :wait-interpolation) ;; 補間をまつ 19 | ;; 20 | (warn "set model's (*dxl-armed-turtlebot*) joint angle to :reset-pose~%~%") 21 | (send *dxl-armed-turtlebot* :reset-pose) ;; 関節角度を :reset-pose にセット 22 | (send *irtviewer* :draw-objects) ;; 描画 23 | (unix::usleep (* 2000 1000)) 24 | ;; 25 | (warn "move real robot (*ri*) with 4000 [ms]~%~%") 26 | (send *ri* :angle-vector (send *dxl-armed-turtlebot* :angle-vector) 4000) ;;4000[ms] で実行 27 | (send *ri* :wait-interpolation) ;; 補間をまつ 28 | ;; 29 | )) 30 | 31 | 32 | (defun dxl-arm-check () 33 | (let ((send-av)) 34 | (send *ri* :angle-vector (send *dxl-armed-turtlebot* :reset-pose) 4000) 35 | (send *irtviewer* :draw-objects) 36 | (warn ";;~%") 37 | (warn ";; go to reset-pose. Check it, then press enter~%") 38 | (warn ";;~%") 39 | (do-until-key 40 | (x::window-main-one)) 41 | ;;arm 42 | (dotimes (i (- *arm-dof* 1)) 43 | (setq send-av (instantiate float-vector *arm-dof*)) 44 | (setf (elt send-av i) (+ (aref send-av i) 50)) 45 | (send *dxl-armed-turtlebot* :angle-vector send-av) 46 | (send *irtviewer* :draw-objects) 47 | (send *ri* :angle-vector (send *dxl-armed-turtlebot* :angle-vector) 4000) 48 | (warn ";;~%") 49 | (format t ";; joint_~A of the robot was moved. Are the real robot and the eusmodel same? Check it, then press enter~%" i) 50 | (warn ";;~%") 51 | (do-until-key 52 | (x::window-main-one)) 53 | (warn ";; Same?~%") 54 | (if (y-or-n-p) 55 | (progn 56 | (send *ri* :angle-vector (send *dxl-armed-turtlebot* :init-pose) 4000) 57 | (send *irtviewer* :draw-objects)) 58 | (progn 59 | (warn ";; fix arm direction!!!~%") 60 | (send *ri* :angle-vector (send *dxl-armed-turtlebot* :init-pose) 4000) 61 | (send *irtviewer* :draw-objects) 62 | (return-from dxl-arm-check)))) 63 | ;; 64 | ;;gripper 65 | (send *dxl-armed-turtlebot* :gripper :arm :angle-vector #f(50)) 66 | (send *irtviewer* :draw-objects) 67 | (send *ri* :move-gripper (send *dxl-armed-turtlebot* :gripper :arm :joint-angle) :tm 1000 :wait t) 68 | (warn ";;~%") 69 | (warn ";; The gripper was moved. Are real robot and model same? Check it, then press enter~%") 70 | (warn ";;~%") 71 | (do-until-key 72 | (x::window-main-one)) 73 | (warn ";; Same?~%") 74 | (if (y-or-n-p) 75 | (progn 76 | (send *dxl-armed-turtlebot* :gripper :arm :angle-vector #f(0)) 77 | (send *ri* :move-gripper (send *dxl-armed-turtlebot* :gripper :arm :joint-angle) :tm 1000 :wait t) 78 | (send *irtviewer* :draw-objects)) 79 | (progn 80 | (warn ";; fix gripper direction!!!~%") 81 | (send *dxl-armed-turtlebot* :gripper :arm :angle-vector #f(0)) 82 | (send *ri* :move-gripper (send *dxl-armed-turtlebot* :gripper :arm :joint-angle) :tm 1000 :wait t) 83 | (send *irtviewer* :draw-objects) 84 | (return-from dxl-arm-check))) 85 | ;; 86 | (warn ";; finish program. back to reset pose~%") 87 | (send *ri* :angle-vector (send *dxl-armed-turtlebot* :reset-pose) 4000) 88 | (send *irtviewer* :draw-objects) 89 | )) 90 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/euslisp/display-bounding-box-array.l: -------------------------------------------------------------------------------- 1 | ;; robotの初期化 2 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") 3 | (dxl-armed-turtlebot-init) 4 | 5 | ;; jsk_pcl_rosのmsgを使うため、loadする 6 | ;;(ros::load-ros-manifest "jsk_pcl_ros") 7 | (ros::roseus-add-msgs "jsk_recognition_msgs") 8 | 9 | ;; クラスタリング結果であるBoundingBoxのtopic名 10 | ;;(defvar *topic-name* "/camera/depth_registered/cluster_decomposer/boxes") 11 | (defvar *topic-name* "/camera/depth_registered/boxes") 12 | (defvar *bounding-box-list* nil) 13 | 14 | ;; ros::initする 15 | (ros::roseus "boundingboxarray_subscriber") 16 | 17 | ;; コールバック関数 18 | (defun bounding-box-array-cb (msg) 19 | (setq *bounding-box-list* (send msg :boxes)) ;; boxesは、BoundingBoxのArray(Euslispではlist) 20 | ;; BoundingBoxがあれば表示する 21 | (when *bounding-box-list* 22 | (send *irtviewer* :draw-objects :flush nil) 23 | (mapcar #'(lambda (b) 24 | ;; BoundingBoxは、dimensions(直方体の幅・奥行き・高さ)をもつ 25 | (let* ((dims (ros::tf-point->pos (send b :dimensions))) 26 | (bx (make-cube (elt dims 0) (elt dims 1) (elt dims 2))) 27 | ;; (1) BoundingBoxのカメラ相対の座標系は、geometry_msgs/Poseという型で得られるので、Euslispのcoordsに変換する 28 | (cam->obj-coords (ros::tf-pose->coords (send b :pose))) 29 | ;; (2) *dxl-armed-turtlebot*モデルがカメラの座標系をもってるので、取得する 30 | (cam-coords (send (send *dxl-armed-turtlebot* :camera_rgb_optical_frame_lk) :copy-worldcoords))) 31 | ;; (3) Euslisp内部でのworld座標系の値にして、そこにmake-cubeの箱を設置する 32 | (send bx :newcoords (send cam-coords :transform cam->obj-coords)) 33 | (send bx :worldcoords) 34 | (send bx :draw-on :flush nil :color #f(1 0 0)) ;; 描画 35 | bx)) 36 | *bounding-box-list*) 37 | (send *irtviewer* :viewer :viewsurface :flush) 38 | )) 39 | 40 | (ros::subscribe *topic-name* jsk_recognition_msgs::BoundingBoxArray #'bounding-box-array-cb 1) 41 | 42 | (do-until-key 43 | (x::window-main-one) ;; IRT viewerの視点を変えられる。見にくければ変えよう 44 | (ros::spin-once) 45 | (ros::sleep) 46 | ) 47 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/euslisp/display-checkerboard.l: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env roseus 2 | 3 | ;; robotの初期化 4 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") 5 | (dxl-armed-turtlebot-init) 6 | 7 | ;; ObjectDetection型トピックを使うため 8 | (ros::roseus-add-msgs "std_msgs") 9 | (ros::roseus-add-msgs "roseus") 10 | (ros::roseus-add-msgs "geometry_msgs") 11 | (ros::roseus-add-msgs "image_view2") 12 | (ros::roseus-add-msgs "posedetection_msgs") 13 | 14 | ;;; 表示モデルなど 15 | (load "models/chessboard-30-7x5-object.l") 16 | (if (not (boundp '*irtviewer*)) (make-irtviewer)) 17 | (setq *target-object* (chessboard-30-7x5 :name "/test_object")) 18 | (objects (list *target-object* *dxl-armed-turtlebot*)) 19 | 20 | (setq *tfb* (instance ros::transform-broadcaster :init)) 21 | 22 | (ros::roseus "objectdetection_client") 23 | 24 | ;; ObjectDetection用コールバック関数定義 25 | (defun objectdetection-cb (msg) 26 | (let ((mrk (instance image_view2::ImageMarker2 :init)) frame-id type ret) 27 | ;; 物体モデルを配置 28 | (setq frame-id (concatenate string "/" (send msg :header :frame_id))) 29 | (mapcar #'(lambda (obj-pose) 30 | (let* (;; (1) カメラ相対の座標系は、geometry_msgs/Poseという型で得られるので、Euslispのcoordsに変換する 31 | (cam->obj-coords (ros::tf-pose->coords (send obj-pose :pose))) 32 | ;; (2) *dxl-armed-turtlebot*モデルがカメラの座標系をもってるので、取得する 33 | (cam-coords (send (send *dxl-armed-turtlebot* :camera_rgb_optical_frame_lk) :copy-worldcoords))) 34 | ;; (3) Euslisp内部でのworld座標系の値にして、そこにcheckerboardモデルを配置する 35 | (send *target-object* :newcoords (send cam-coords :transform cam->obj-coords)) 36 | )) 37 | (send msg :objects)) 38 | ;; image_markerを出力 39 | (dolist (obj-pose (send msg :objects)) 40 | (setq type (send obj-pose :type)) 41 | (unless (eq (char type 0) #\/) (setq type (concatenate string "/" type))) 42 | (setq ret (ros::tf-pose->coords (send obj-pose :pose))) 43 | (send mrk :type image_view2::ImageMarker2::*FRAMES*) 44 | (send mrk :frames (list type)) 45 | (send *tfb* :send-transform ret frame-id type) 46 | (ros::ros-info "~A ~A ~A" ret frame-id type) 47 | (ros::publish "image_marker" mrk)) 48 | )) 49 | 50 | (ros::advertise "image_marker" image_view2::ImageMarker2 1) 51 | (ros::subscribe "/camera/rgb/ObjectDetection" posedetection_msgs::ObjectDetection #'objectdetection-cb) 52 | 53 | (ros::rate 10) 54 | (do-until-key 55 | (ros::spin-once) 56 | (send *irtviewer* :draw-objects) 57 | (x::window-main-one) 58 | (ros::sleep) 59 | ) 60 | 61 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l: -------------------------------------------------------------------------------- 1 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 2 | ;; アーム台車ロボットのrobot-interfaceクラス 3 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 4 | 5 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l") 6 | (require :robot-interface "package://pr2eus/robot-interface.l") 7 | 8 | ;; loadする台車・アームのrobot-interfaceクラスのメソッド定義ファイル 9 | (load "package://turtleboteus/euslisp/turtlebot-interface-common.l") 10 | (load "package://dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface-common.l") 11 | 12 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 13 | ;; アーム台車ロボットのrobot-interfaceクラス定義 14 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 15 | (defclass dxl-armed-turtlebot-interface 16 | :super robot-interface 17 | :slots (gripper-action)) 18 | 19 | (eval `(defmethod dxl-armed-turtlebot-interface 20 | ;; dxl-7dof-arm-interface, turtlebot-interfaceのメソッドをそれぞれdefmethodする 21 | ,@(get-method-list-for-dxl-7dof-arm-interface) 22 | ,@(get-method-list-for-turtlebot-interface) 23 | )) 24 | (defmethod dxl-armed-turtlebot-interface 25 | (:init 26 | (&rest args) 27 | (prog1 28 | (send-super* :init :robot dxl-armed-turtlebot-robot args) 29 | ;; それぞれのcallback登録など 30 | (send self :initialize-turtlebot-ros) 31 | (send self :initialize-arm-robot-ros) 32 | ) 33 | ) 34 | ) 35 | 36 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 37 | ;; アーム台車ロボット用初期化関数 38 | ;; robot-interface (*ri*) とモデル (*dxl-armed-turtlebot*)を生成する 39 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 40 | (defun dxl-armed-turtlebot-init 41 | (&key (objects)) 42 | "Initialization function for *ri* and *dxl-armed-turtlebot*." 43 | (if (not (boundp '*ri*)) 44 | (setq *ri* (instance dxl-armed-turtlebot-interface :init :objects objects))) 45 | (if (not (boundp '*dxl-armed-turtlebot*)) 46 | (setq *dxl-armed-turtlebot* (dxl-armed-turtlebot))) 47 | (objects (list *dxl-armed-turtlebot*)) 48 | (send *irtviewer* :change-background #f(0.9 0.9 0.9)) 49 | (send *irtviewer* :draw-objects) 50 | ) 51 | (warn ";; (dxl-armed-turtlebot-init) ;; for initialize ~%") 52 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/euslisp/gazebo-grasp-test.l: -------------------------------------------------------------------------------- 1 | ;; robotの初期化 2 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") 3 | (dxl-armed-turtlebot-init) 4 | 5 | ;; arm approach pose 6 | (send *ri* :angle-vector #f(0 0 -90 0 90 0) 3000) 7 | (send *ri* :wait-interpolation) 8 | 9 | ;; open gripper 10 | (send *ri* :stop-grasp) 11 | 12 | ;; go-pos 13 | (send *ri* :go-pos 0.56 0 1) 14 | 15 | ;; grasp 16 | ;; still some bug in start-grasp, so we have to directly use :move-gripper 17 | (send *ri* :move-gripper -30 :tm 1500 :wait t) 18 | (send *ri* :wait-interpolation) 19 | (send *ri* :angle-vector #f(90 0 45 0 45 45)) ;; arm tuck pose 20 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/euslisp/gazebo-grasp-with-recognition.l: -------------------------------------------------------------------------------- 1 | ;; robotの初期化 2 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") 3 | (dxl-armed-turtlebot-init) 4 | 5 | (ros::load-ros-manifest "jsk_recognition_msgs") 6 | 7 | (defvar *topic-name* "/camera/depth/boxes") 8 | (defvar *bounding-box-list* nil) 9 | 10 | (setq found-obj nil) 11 | (setq obj-pos #f(0 0 0)) 12 | 13 | ;; ros::initする 14 | (ros::roseus "boundingboxarray_subscriber") 15 | 16 | ;; コールバック関数 17 | (defun bounding-box-array-cb (msg) 18 | (setq *bounding-box-list* (send msg :boxes)) ;; boxesは、BoundingBoxのArray(Euslispではlist) 19 | (when *bounding-box-list* 20 | (let* ((b (elt *bounding-box-list* 0)) 21 | (cam->obj-coords (ros::tf-pose->coords (send b :pose))) 22 | (cam-coords (send (send *dxl-armed-turtlebot* :camera_depth_optical_frame_lk) :copy-worldcoords))) 23 | (setq obj-pos (scale 0.001 (send (send cam-coords :transform cam->obj-coords) :worldpos))) 24 | (setq found-obj t) 25 | obj-pos)) 26 | ) 27 | 28 | (ros::subscribe *topic-name* jsk_recognition_msgs::BoundingBoxArray #'bounding-box-array-cb 1) 29 | 30 | (setq found-obj nil) 31 | (until found-obj 32 | (x::window-main-one) ;; IRT viewerの視点を変えられる。見にくければ変えよう 33 | (ros::spin-once) 34 | (ros::sleep) 35 | ) 36 | 37 | (ros::ros-warn "found target ojbect ~A, appoaching" obj-pos) 38 | 39 | (setq 2d-pos (float-vector (elt obj-pos 0) (elt obj-pos 1))) 40 | 41 | (send *ri* :go-pos 42 | (elt (scale (/ (- (norm 2d-pos) 0.6) (norm 2d-pos)) 2d-pos) 0) 43 | (elt (scale (/ (- (norm 2d-pos) 0.6) (norm 2d-pos)) 2d-pos) 1) 44 | (rad2deg (atan (elt obj-pos 1) (elt obj-pos 0)))) 45 | 46 | ;; open gripper 47 | (send *ri* :stop-grasp) 48 | 49 | 50 | (setq found-obj nil) 51 | (until found-obj 52 | (x::window-main-one) ;; IRT viewerの視点を変えられる。見にくければ変えよう 53 | (ros::spin-once) 54 | (ros::sleep) 55 | ) 56 | 57 | (ros::ros-warn "re-found target object ~A" obj-pos) 58 | (setq target-cds (make-coords :pos (scale 1000 obj-pos))) 59 | (send target-cds :translate #f(-200 0 50)) ;;z should be 0, but the link is not rigid in gazebo, so 100 is the height offset for end effector. 60 | (objects (list *dxl-armed-turtlebot* target-cds)) 61 | 62 | (send *dxl-armed-turtlebot* :angle-vector #f(0 0 -90 0 90 0)) 63 | 64 | (send *dxl-armed-turtlebot* :arm :end-coords :translate #f(0 30 0)) ;; change the end-effector position 65 | 66 | (send *dxl-armed-turtlebot* :inverse-kinematics target-cds :rotation-axis :y) 67 | 68 | #| 69 | (send *dxl-armed-turtlebot* :angle-vector (map float-vector #'(lambda(ang) 70 | (cond 71 | ((> ang 90) (- ang 180)) 72 | ((< ang -90) (+ ang 180)) 73 | (t ang) 74 | )) 75 | (send *dxl-armed-turtlebot* :angle-vector))) 76 | |# 77 | 78 | (ros::ros-warn "the grap arm pose is ~A" (send *dxl-armed-turtlebot* :angle-vector)) 79 | 80 | (send *ri* :angle-vector (send *dxl-armed-turtlebot* :angle-vector) 3000) 81 | (send *ri* :wait-interpolation) 82 | 83 | (send *ri* :go-pos 0.1 0 0) 84 | 85 | ;; grasp 86 | ;; still some bug in start-grasp, so we have to directly use :move-gripper 87 | (send *ri* :move-gripper -30 :tm 1500 :wait t) 88 | (send *ri* :wait-interpolation) 89 | 90 | (send *ri* :angle-vector #f(80 0 45 0 45 45)) ;; arm tuck pose 91 | (send *ri* :go-pos -0.6 0 0) ;; arm tuck pose 92 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/euslisp/joy-sample.l: -------------------------------------------------------------------------------- 1 | ;; joy sample for turtlebot ensyu 2 | 3 | (load "package://roseus/test/joy.l") 4 | (init-ps3joy) ;; *joy* という大域変数のセット 5 | 6 | (progn 7 | (ros::rate 10) 8 | (warn "~%press circle button~%") 9 | (do-until-key 10 | (ros::spin-once) ;; subscribe をよんで, joyというtopic を読み込む 11 | (ros::sleep) 12 | (if (= (send *joy* :circle-button) 1) ;; joyの結果から,丸ボタンの値だけ抜き出す 13 | (print "circle button pressed")) 14 | ) 15 | ) 16 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/euslisp/keyboard-ik-sample.l: -------------------------------------------------------------------------------- 1 | ;;; 2 | ;;; 移動台車モデルの逆運動学を解いてarmや台車を操縦するサンプル 3 | ;;; 4 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") 5 | 6 | (dxl-armed-turtlebot-init) 7 | 8 | #|********************************************* 9 | キーボード入力によって目標座標を少しずつ動かすサンプル 10 | ***********************************************|# 11 | (warn "(ik-demo0)~%") 12 | (defun ik-demo0 13 | (&key (robot *dxl-armed-turtlebot*)) 14 | ;;逆運動学が解きやすい初期姿勢に変更 15 | (warn ";; move to reset-pose~%") 16 | (send robot :reset-pose) 17 | (when (boundp '*ri*) 18 | (send *ri* :angle-vector (send robot :angle-vector) 5000) 19 | (send *ri* :wait-interpolation)) 20 | (objects (list robot)) 21 | 22 | ;; 23 | ;;'e'を押すまで続ける 24 | (warn ";; if stop, then enter e~%") 25 | (warn ";; h:left, j:down, k:up, l:right, f:forward, b:back~%") 26 | (let (w goal-endcoords ll) 27 | ;;もし腕しか使わない場合はlinklistをあらかじめ用意しておく 28 | ;;目標座標を作成する(デフォルトは手先位置と同じにする) 29 | (setq goal-endcoords 30 | (make-cascoords :pos (send (send robot :arm :end-coords :copy-worldcoords) :worldpos))) 31 | ;;ループを回す 32 | (while t 33 | (setq w (read-line)) ;;文字を取得 34 | ;;文字によって操作を変える 35 | (cond 36 | ((equal w "e") 37 | (return-from nil)) ;;loopから抜けて終了 38 | ((equal w "h") ;;左へ動かす 39 | (send goal-endcoords :locate #f(0 20 0) :local)) 40 | ((equal w "j") ;;下へ動かす 41 | (send goal-endcoords :locate #f(0 0 -20) :local)) 42 | ((equal w "k") ;;上へ動かす 43 | (send goal-endcoords :locate #f(0 0 20) :local)) 44 | ((equal w "l") ;;右へ動かす 45 | (send goal-endcoords :locate #f(0 -20 0) :local)) 46 | ((equal w "f") ;;前へ動かす 47 | (send goal-endcoords :locate #f( 20 0 0) :local)) 48 | ((equal w "b") ;;後へ動かす 49 | (send goal-endcoords :locate #f(-20 0 0) :local)) 50 | ((not w)) ;;何も入れられなければ何もしない 51 | (t 52 | (warn ";; no such command~%") 53 | (warn ";; if stop, then enter e~%") 54 | (warn ";; h:left, j:down, k:up, l:right, f:forward, b:back~%") 55 | )) 56 | 57 | ;;目標値goal-endcoordsに向かって逆運動学を解く． 58 | ;; inverse-kinematicsという逆運動学を解くmethodを呼び出す． 59 | (send *dxl-armed-turtlebot* :inverse-kinematics goal-endcoords :rotation-axis nil) 60 | 61 | (send *irtviewer* :objects (list robot goal-endcoords)) 62 | (send *irtviewer* :draw-objects) 63 | 64 | ;;angle-vectorで逆運動学を解いた姿勢に移行する 65 | (when (boundp '*ri*) 66 | (send *ri* :angle-vector (send robot :angle-vector) 500) 67 | (send *ri* :wait-interpolation) 68 | ) 69 | )) 70 | (warn ";; finished~%") 71 | t) 72 | 73 | (ik-demo0) 74 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/euslisp/realtime-ik-sample.l: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env roseus 2 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") 3 | 4 | (ros::roseus "realtime_ik_node") 5 | 6 | (dxl-armed-turtlebot-init) 7 | (setq *robot* *dxl-armed-turtlebot*) 8 | (setq arm-tgt (make-coords :pos #f(300 0 200))) 9 | 10 | (objects *robot*) 11 | 12 | (defclass dxl-armed-turtlebot-ik-class 13 | :super propertied-object 14 | :slots ()) 15 | (defmethod dxl-armed-turtlebot-ik-class 16 | (:init 17 | () 18 | (ros::subscribe "ik_arm_tgt" geometry_msgs::PoseStamped #'send self :arm-cb) 19 | ) 20 | (:arm-cb 21 | (msg) 22 | (setq arm-tgt (ros::tf-pose->coords (send msg :pose)))) 23 | ) 24 | 25 | (setq m (instance dxl-armed-turtlebot-ik-class :init)) 26 | 27 | (defun run () 28 | (send *ri* :angle-vector (send *robot* :reset-pose) 2000) 29 | (send *ri* :wait-interpolation) 30 | 31 | (ros::rate 1) 32 | (while (ros::ok) 33 | (send *robot* :inverse-kinematics arm-tgt 34 | :translation-axis t :rotation-axis nil 35 | :stop 2 36 | :revert-if-fail nil :debug-view nil 37 | ) 38 | (send *ri* :angle-vector (send *robot* :angle-vector) 1000) 39 | 40 | (send arm-tgt :draw-on :flush t :color #f(1 0 0)) 41 | (send *irtviewer* :draw-objects :flush nil) 42 | (x::window-main-one) 43 | (ros::spin-once) 44 | (ros::sleep) 45 | ) 46 | ) 47 | 48 | (run) 49 | 50 | 51 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/images/gazebo_checkerboard.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/dxl_armed_turtlebot/images/gazebo_checkerboard.png -------------------------------------------------------------------------------- /dxl_armed_turtlebot/images/gazebo_turtlebot2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/dxl_armed_turtlebot/images/gazebo_turtlebot2.png -------------------------------------------------------------------------------- /dxl_armed_turtlebot/images/gazebo_turtlebot_grasp.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/dxl_armed_turtlebot/images/gazebo_turtlebot_grasp.png -------------------------------------------------------------------------------- /dxl_armed_turtlebot/images/gazebo_turtlebot_slam.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/dxl_armed_turtlebot/images/gazebo_turtlebot_slam.png -------------------------------------------------------------------------------- /dxl_armed_turtlebot/kadai/checkpoint1-3-1-button.l: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env roseus 2 | 3 | (ros::roseus-add-msgs "kobuki_msgs") 4 | 5 | (ros::roseus "checkpoint1_3_1_button_euslisp") 6 | 7 | (defun button-cb 8 | (msg) 9 | (setq button (send msg :button)) 10 | (setq state (send msg :state)) 11 | (ros::ros-info "subscribe msg [button: ~A state: ~A]" button state) 12 | ) 13 | 14 | (ros::subscribe "/mobile_base/events/button" 15 | kobuki_msgs::ButtonEvent #'button-cb) 16 | 17 | (ros::ros-info "ready to subscribe !!!") 18 | (ros::spin) 19 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/kadai/checkpoint1-3-2-led.l: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env roseus 2 | 3 | (ros::roseus-add-msgs "kobuki_msgs") 4 | 5 | (ros::roseus "checkpoint1_3_2_led_euslisp") 6 | 7 | (ros::advertise "/mobile_base/commands/led1" kobuki_msgs::Led) 8 | (unix::sleep 1) 9 | 10 | (setq msg (instance kobuki_msgs::Led :init)) 11 | (dotimes (i 3) 12 | (send msg :value (+ i 1)) 13 | (ros::ros-info "publish msg [value: ~A]" (send msg :value)) 14 | (ros::publish "/mobile_base/commands/led1" msg) 15 | (unix::sleep 1) 16 | ) 17 | (send msg :value 0) 18 | (ros::ros-info "publish [value: ~A]" (send msg :value)) 19 | (ros::publish "/mobile_base/commands/led1" msg) 20 | 21 | (exit) 22 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/kadai/checkpoint1_3_1_button.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | from kobuki_msgs.msg import ButtonEvent 5 | 6 | def button_cb(msg): 7 | button = msg.button 8 | state = msg.state 9 | rospy.loginfo("subscribe msg [button: %d state: %d]"%(button, state)) 10 | 11 | if __name__ == '__main__': 12 | rospy.init_node("checkpoint1_3_1_button_python", anonymous=True) 13 | 14 | rospy.Subscriber("/mobile_base/events/button", ButtonEvent, button_cb) 15 | 16 | rospy.spin() 17 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/kadai/checkpoint1_3_2_led.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | from kobuki_msgs.msg import Led 5 | 6 | if __name__ == '__main__': 7 | rospy.init_node("checkpoint1_3_2_led_python", anonymous=True) 8 | 9 | pub = rospy.Publisher("/mobile_base/commands/led1", Led, queue_size=1) 10 | rospy.sleep(1) 11 | 12 | msg = Led() 13 | for i in range(3): 14 | msg.value = i+1 15 | rospy.loginfo("publish msg [value: %d]"%(msg.value)) 16 | pub.publish(msg) 17 | rospy.sleep(1) 18 | msg.value = 0 19 | rospy.loginfo("publish msg [value: %d]"%(msg.value)) 20 | pub.publish(msg) 21 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/kadai/checkpoint2-1-go-forward.l: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env roseus 2 | 3 | (load "package://turtleboteus/euslisp/turtlebot-interface") 4 | 5 | (ros::roseus "checkpoint2_1_go_forward") 6 | 7 | (turtlebot-init) 8 | 9 | (ros::ros-info "start !!!") 10 | 11 | (while t 12 | (send *ri* :go-velocity 0.1 0 0) 13 | (when (not (= (norm (send *ri* :state :bumper-vector)) 0)) ;; ひとつでも非ゼロなら 14 | (send *ri* :go-velocity 0 0 0) 15 | (return-from nil nil))) 16 | 17 | (exit) 18 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/kadai/checkpoint2-3-joy-ik.l: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env roseus 2 | 3 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") 4 | (load "package://roseus/test/joy.l") 5 | 6 | (ros::roseus "checkpoint2_3_joy_ik") 7 | 8 | (dxl-armed-turtlebot-init) 9 | (init-ps3joy) 10 | 11 | (ros::ros-info "send initial pose.") 12 | (send *dxl-armed-turtlebot* :reset-pose) 13 | (send *ri* :angle-vector (send *dxl-armed-turtlebot* :angle-vector) 3000) 14 | (send *ri* :wait-interpolation) 15 | 16 | (ros::ros-info "start !!!") 17 | (ros::rate 10) 18 | (do-until-key ;; Enterでループを抜ける 19 | (when (= (send *joy* :circle-button) 1) 20 | (ros::ros-info "circle button pushed.") 21 | (send *dxl-armed-turtlebot* :arm :move-end-pos 22 | (float-vector 0 0 20) :world :rotation-axis nil) ;; 手先を20mm上に移動 23 | (send *irtviewer* :draw-objects) 24 | (send *ri* :angle-vector (send *dxl-armed-turtlebot* :angle-vector) 1000) 25 | (send *ri* :wait-interpolation) 26 | ) 27 | (x::window-main-one) 28 | (ros::sleep) 29 | (ros::spin-once) 30 | ) 31 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/kadai/kadai2_3_cmd_vel.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import math 4 | import rospy 5 | from geometry_msgs.msg import Twist 6 | from opencv_apps.msg import RotatedRectStamped 7 | 8 | pub = None 9 | 10 | def cb(msg): 11 | global pub 12 | cmd_vel = Twist() 13 | size = msg.rect.size.width * msg.rect.size.height 14 | rospy.loginfo("rect size: %d\n"%size) 15 | if (size < 30000): 16 | cmd_vel.angular.z = 0.5 17 | cmd_vel.angular.z 18 | pub.publish(cmd_vel) 19 | 20 | if __name__ == '__main__': 21 | try: 22 | rospy.init_node("kadai2_3_cmd_vel", anonymous=True) 23 | rospy.Subscriber("/camshift/track_box", RotatedRectStamped, cb) 24 | pub = rospy.Publisher("/cmd_vel", Twist, queue_size=1) 25 | rospy.spin() 26 | except rospy.ROSInterruptException: pass 27 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/kadai/kadai2_3_joint_command.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import math 4 | import rospy 5 | from trajectory_msgs.msg import JointTrajectory 6 | from trajectory_msgs.msg import JointTrajectoryPoint 7 | 8 | def send_joint_position(): 9 | rospy.init_node("kadai2_3_joint_command", anonymous=True) 10 | 11 | pub = rospy.Publisher("/fullbody_controller/command", JointTrajectory, queue_size=1) 12 | rospy.sleep(1) 13 | 14 | joint_trajectory = JointTrajectory() 15 | joint_trajectory.header.stamp = rospy.Time.now() 16 | joint_trajectory.joint_names = ["arm_joint1", "arm_joint2", "arm_joint3", 17 | "arm_joint4", "arm_joint5", "arm_joint6"] 18 | for i in range(5): 19 | point = JointTrajectoryPoint() 20 | point.positions = [math.pi/2, 0, math.pi/4*(i%2), 0, math.pi/2, math.pi/2] 21 | point.time_from_start = rospy.Duration(1.0+i) 22 | joint_trajectory.points.append(point) 23 | 24 | pub.publish(joint_trajectory) 25 | 26 | rospy.sleep(5) 27 | 28 | if __name__ == '__main__': 29 | try: 30 | send_joint_position() 31 | except rospy.ROSInterruptException: pass 32 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/launch/checkerboard_detector.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/launch/dxl_armed_turtlebot_bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 20 | 21 | 23 | 24 | 25 | 26 | 29 | 30 | 32 | 33 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/launch/dxl_armed_turtlebot_gazebo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 17 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 34 | 35 | 36 | 37 | 38 | 39 | 42 | 43 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/launch/dxl_armed_turtlebot_navigation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/launch/hsi_color_filter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 35 | 36 | 37 | 38 | use_indices: false 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 49 | 50 | 51 | tolerance: 0.02 52 | min_size: 100 53 | 54 | 55 | 56 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/launch/includes/amcl.launch.xml: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/launch/sample_head_arm_skin_detect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/launch/turtlebot_joystick_teleop.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b dxl_armed_turtlebot 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | dxl_armed_turtlebot 3 | 7.0.0 4 | Turtlebot with dynamixel 7dof arm 5 | BSD 6 | Shunichi Nozawa 7 | Shunichi Nozawa 8 | 9 | catkin 10 | 11 | dynamixel_7dof_arm 12 | turtleboteus 13 | roseus 14 | rostest 15 | rqt_joint_trajectory_controller 16 | teleop_twist_keyboard 17 | 18 | controller_manager 19 | depthimage_to_laserscan 20 | dynamixel_7dof_arm 21 | gazebo_plugins 22 | gazebo_ros 23 | gazebo_ros_control 24 | image_view2 25 | joint_state_controller 26 | joint_trajectory_controller 27 | joy 28 | jsk_perception 29 | jsk_pcl_ros 30 | jsk_tools 31 | ps3joy 32 | kobuki_dashboard 33 | kobuki_gazebo 34 | libbluetooth-dev 35 | libjack-dev 36 | map_server 37 | nodelet 38 | opencv_apps 39 | ps3joy 40 | robot_state_publisher 41 | rqt_reconfigure 42 | rviz 43 | topic_tools 44 | turtlebot_bringup 45 | turtlebot_description 46 | turtlebot_navigation 47 | turtlebot_teleop 48 | turtleboteus 49 | turtlebot_navigation 50 | turtlebot_interactive_markers 51 | turtlebot_rviz_launchers 52 | rqt_joint_trajectory_controller 53 | rqt_robot_steering 54 | teleop_twist_keyboard 55 | roseus_tutorials 56 | slam_karto 57 | roseus 58 | openni_launch 59 | cart_humanoid 60 | 61 | 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/scripts/install_ps3joy_daemon.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | sudo ln -sf `rospack find dxl_armed_turtlebot`/scripts/ps3joy /etc/init.d/ps3joy 4 | sudo update-rc.d ps3joy defaults 99 99 # install 5 | #sudo update-rc.d -f ps3joy remove # uninstall 6 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/scripts/install_turtlebot_simulator.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | trap 'exit 1' ERR 4 | set -x 5 | 6 | # 1. gazebo_ros_pkgs インストール 7 | # http://gazebosim.org/wiki/Tutorials/1.9/Installing_gazebo_ros_Packages 8 | # を参照 9 | 10 | # 1-1. Remove ROS's Old Version of Gazebo ROS Integration 11 | echo "1-1. Remove ROS's Old Version of Gazebo ROS Integration" 12 | sudo apt-get remove ros-fuerte-simulator-gazebo ros-groovy-simulator-gazebo 13 | 14 | # 1-2. Install from Source (on Ubuntu) 通りに以下を実行 15 | echo "1-2. Install from Source (on Ubuntu) 通りに以下を実行" 16 | source /opt/ros/groovy/setup.bash 17 | mkdir -p ~/catkin_ws/src 18 | cd ~/catkin_ws/src 19 | catkin_init_workspace 20 | cd ~/catkin_ws 21 | catkin_make 22 | 23 | # 1-3. ここだけWebページとは違う 24 | #echo "1-3. ここだけWebページとは違う" 25 | #echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc 26 | cd ~/ros/groovy/ 27 | rosws merge ~/catkin_ws/devel/.rosinstall 28 | 29 | # 1-4. git clone 30 | echo "1-4. git clone" 31 | source ~/.bashrc 32 | cd ~/catkin_ws/src 33 | git clone https://github.com/ros-simulation/gazebo_ros_pkgs.git 34 | git clone https://github.com/ros-controls/ros_control.git 35 | git clone https://github.com/ros-controls/ros_controllers.git -b groovy-backported-hydro 36 | git clone https://github.com/ros-controls/control_toolbox.git 37 | git clone https://github.com/ros-controls/realtime_tools.git 38 | 39 | # 1-5. 必要に応じて 40 | echo "1-5. 必要に応じて" 41 | cd ~/catkin_ws/src 42 | rosdep update 43 | rosdep check --from-paths . --ignore-src --rosdistro groovy 44 | rosdep install --from-paths . --ignore-src --rosdistro groovy -y 45 | 46 | # 1-6. catkin_makeを実行 47 | echo "1-6. catkin_makeを実行" 48 | source ~/.bashrc 49 | cd ~/catkin_ws/ 50 | catkin_make 51 | #もしpcl...cmakeがないというエラーが出た場合は、 52 | #sudo aptitude install ros-groovy-pcl-conversions 53 | #する 54 | 55 | # gazebo_ros_pkgs 動作確認 56 | #http://gazebosim.org/wiki/Tutorials/1.9/Using_roslaunch_Files_to_Spawn_Models 57 | #roslaunch gazebo_ros willowgarage_world.launch 58 | 59 | 60 | # 2. turtlebot実機インストール 61 | echo "2. turtlebot実機インストール" 62 | sudo apt-get install ros-groovy-turtlebot ros-groovy-turtlebot-apps ros-groovy-turtlebot-viz 63 | 64 | 65 | # 3. turtlebot_simulator関係 66 | # 3-1. kobukiのgazebo plugins 67 | echo "3-1. kobukiのgazebo plugins" 68 | git clone https://github.com/yujinrobot/kobuki_desktop.git ~/catkin_ws/src/kobuki_desktop 69 | cd ~/catkin_ws 70 | catkin_make 71 | 72 | # 3-2. kobuki_description, turtlebot_description 73 | echo "3-2. kobuki_description" 74 | git clone --no-checkout https://github.com/yujinrobot/kobuki.git ~/catkin_ws/src/kobuki 75 | cd ~/catkin_ws/src/kobuki 76 | git reset HEAD kobuki.rosinstall README.markdown kobuki_description || echo "" # ignore error 77 | git checkout kobuki.rosinstall README.markdown kobuki_description 78 | echo "3-2. turtlebot_description" 79 | git clone --no-checkout https://github.com/turtlebot/turtlebot.git ~/catkin_ws/src/turtlebot 80 | cd ~/catkin_ws/src/turtlebot 81 | git reset HEAD README.md setup_kobuki.sh turtlebot.rosinstall setup_create.sh turtlebot_description || echo "" # ignore error 82 | git checkout README.md setup_kobuki.sh turtlebot.rosinstall setup_create.sh turtlebot_description 83 | 84 | # 3-3. turtlebot_simulator 85 | echo "3-3. turtlebot_simulator" 86 | git clone https://github.com/turtlebot/turtlebot_simulator.git ~/catkin_ws/src/turtlebot_simulator 87 | # patching 88 | sed -i -e 's/bumper2pc.launch.xml/_bumper2pc.launch/g' ~/catkin_ws/src/turtlebot_simulator/turtlebot_gazebo/launch/includes/kobuki.launch.xml 89 | 90 | # 3-4. for catkin_make of xx_description 91 | echo "3-4. for catkin_make of xx_description" 92 | git clone -b hydro-devel https://github.com/ros/xacro.git ~/catkin_ws/src/xacro 93 | cd ~/catkin_ws/ 94 | catkin_make 95 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/scripts/ps3joy: -------------------------------------------------------------------------------- 1 | ### BEGIN INIT INFO 2 | # Provides: ps3joy 3 | # Required-Start: 4 | # Required-Stop: 5 | # Default-Start: 2 3 4 5 6 | # Default-Stop: 0 1 6 7 | # Short-Description: starts instance of ps3joy.py 8 | # Description: starts instance of pd3joy.py 9 | ### END INIT INFO 10 | 11 | ############### EDIT ME ################## 12 | 13 | #APP_PATH="" 14 | 15 | #. /opt/ros/electric/setup.sh 16 | #. /opt/ros/groovy/setup.sh 17 | #. /opt/ros/hydro/setup.sh 18 | . /opt/ros/indigo/setup.sh 19 | export ROS_HOME=/root 20 | 21 | # path to daemon 22 | #DAEMON=`rospack find ps3joy`/ps3joy.py 23 | DAEMON=/opt/ros/$ROS_DISTRO/lib/ps3joy/ps3joy.py 24 | 25 | # startup args 26 | DAEMON_OPTS="--inactivity-timeout=900 --redirect-output=/var/log/ps3joy.log" 27 | 28 | # script name 29 | NAME=ps3joy 30 | 31 | # app name 32 | DESC=ps3joy 33 | 34 | RUN_AS=root 35 | 36 | PID_FILE=/var/run/ps3joy.pid 37 | 38 | ############### END EDIT ME ################## 39 | 40 | test -x $DAEMON || exit 0 41 | 42 | set -e 43 | 44 | case "$1" in 45 | start) 46 | echo -n "Starting $DESC: " 47 | 48 | start-stop-daemon -c $RUN_AS --start --background --pidfile $PID_FILE --make-pidfile -a $DAEMON -- $DAEMON_OPTS 49 | 50 | if [ $? ]; then 51 | echo "[OK]" 52 | else 53 | echo "[FAILED]" 54 | fi 55 | 56 | ;; 57 | stop) 58 | echo -n "Stopping $DESC: " 59 | start-stop-daemon --stop --pidfile $PID_FILE 60 | if [ $? ]; then 61 | echo "[OK]" 62 | else 63 | echo "[FAILED]" 64 | fi 65 | ;; 66 | 67 | restart|force-reload) 68 | echo -n "Restarting $DESC: " 69 | start-stop-daemon --stop --oknodo --pidfile $PID_FILE 70 | sleep 1 71 | start-stop-daemon -c $RUN_AS --start --background --pidfile $PID_FILE --make-pidfile -a $DAEMON -- $DAEMON_OPTS 72 | if [ $? ]; then 73 | echo "[OK]" 74 | else 75 | echo "[FAILED]" 76 | fi 77 | 78 | ;; 79 | *) 80 | N=/etc/init.d/$NAME 81 | echo "Usage: $N {start|stop|restart|force-reload}" >&2 82 | exit 1 83 | ;; 84 | esac 85 | 86 | exit 0 87 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/scripts/tuck_arm.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | 4 | import rospy 5 | import argparse 6 | from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint 7 | from control_msgs.msg import JointTrajectoryControllerState 8 | from math import pi 9 | 10 | class Tuck(object): 11 | def __init__(self, tuck_cmd): 12 | self._done = False 13 | self._tuck = tuck_cmd 14 | self._tuck_rate = rospy.Rate(5.0) # Hz 15 | self._tuck_threshold = 0.3 # radians 16 | self._joint_moves = { 17 | 'tuck': [pi/2, 0, pi/4, 0, pi/2, pi/2], 18 | 'untuck': [0, 0, -pi/2, 0, pi/2, 0], 19 | } 20 | self._arm_state = 'none' 21 | self._pub = rospy.Publisher('/fullbody_controller/command', JointTrajectory, queue_size=1) 22 | self._sub = rospy.Subscriber('/fullbody_controller/state', JointTrajectoryControllerState, self._check_arm_state) 23 | 24 | def _check_arm_state(self, msg): 25 | diff_check = lambda a, b: abs(a - b) <= self._tuck_threshold 26 | angles = msg.actual.positions 27 | tuck_goal = map(diff_check, angles, self._joint_moves['tuck']) 28 | untuck_goal = map(diff_check, angles, self._joint_moves['untuck']) 29 | 30 | if all(tuck_goal): 31 | self._arm_state = 'tuck' 32 | elif all(untuck_goal): 33 | self._arm_state = 'untuck' 34 | else: 35 | self._arm_state = 'none' 36 | 37 | def _prepare_to_tuck(self): 38 | pass 39 | 40 | def _move_to(self, goal): 41 | traj = JointTrajectory() 42 | traj.joint_names = [ 43 | 'arm_joint1', 44 | 'arm_joint2', 45 | 'arm_joint3', 46 | 'arm_joint4', 47 | 'arm_joint5', 48 | 'arm_joint6', 49 | ] 50 | traj.points.append(JointTrajectoryPoint()) 51 | traj.points[0].positions = self._joint_moves[goal] 52 | traj.points[0].time_from_start = rospy.Duration(3) 53 | 54 | while self._arm_state != goal: 55 | self._pub.publish(traj) 56 | self._tuck_rate.sleep() 57 | 58 | def supervised_tuck(self): 59 | # Update our starting state to check if arms are tucked 60 | self._prepare_to_tuck() 61 | # Tuck Arms 62 | if self._tuck == True: 63 | # If arms are already tucked, report this to user and exit. 64 | if self._arm_state == 'tuck': 65 | rospy.loginfo("Tucking: Arm already in 'Tucked' position.") 66 | else: 67 | rospy.loginfo("Tucking: arm is not Tucked.") 68 | self._move_to('tuck') 69 | self._done = True 70 | return 71 | 72 | # Untuck Arms 73 | else: 74 | # If arms already untucked 75 | if self._arm_state == 'untuck': 76 | rospy.loginfo("Untucking: Arm already Untucked") 77 | else: 78 | rospy.loginfo("Untucking: arm is not Untucked.") 79 | self._move_to('untuck') 80 | self._done = True 81 | return 82 | 83 | def clean_shutdown(self): 84 | if not self._done: 85 | rospy.logwarn('Aborting: Shutting down safely...') 86 | 87 | 88 | def main(): 89 | parser = argparse.ArgumentParser() 90 | tuck_group = parser.add_mutually_exclusive_group(required=True) 91 | tuck_group.add_argument("-t", "--tuck", dest="tuck", 92 | action='store_true', default=False, help="tuck arms") 93 | tuck_group.add_argument("-u", "--untuck", dest="untuck", 94 | action='store_true', default=False, help="untuck arms") 95 | args = parser.parse_args(rospy.myargv()[1:]) 96 | tuck = args.tuck 97 | 98 | rospy.loginfo("Initializing node... ") 99 | rospy.init_node("tuck_arms") 100 | rospy.loginfo("%sucking arms" % ("T" if tuck else "Unt",)) 101 | tucker = Tuck(tuck) 102 | rospy.on_shutdown(tucker.clean_shutdown) 103 | tucker.supervised_tuck() 104 | rospy.loginfo("Finished %suck" % ("T" if tuck else "Unt",)) 105 | 106 | if __name__ == "__main__": 107 | main() 108 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/test/test-dxl-armed-turtlebot.l: -------------------------------------------------------------------------------- 1 | #+:ros (require :unittest "lib/llib/unittest.l") 2 | 3 | (unless *unit-test* 4 | (init-unit-test)) 5 | 6 | (deftest test-init-model 7 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot.l") 8 | (assert (setq *dxl-armed-turtlebot* (dxl-armed-turtlebot))) 9 | (assert (objects (list *dxl-armed-turtlebot*))) 10 | (dolist (pose (remove-if-not #'(lambda (x) (substringp "pose" (string-downcase x))) (send *dxl-armed-turtlebot* :methods))) 11 | (assert (send *dxl-armed-turtlebot* pose))) 12 | (send *dxl-armed-turtlebot* :tuckarm-pose) 13 | (assert (send *dxl-armed-turtlebot* :arm :move-end-pos (float-vector 20 20 -20) :world :rotation-axis nil)) 14 | ) 15 | 16 | (deftest test-init-ri 17 | (load "package://dxl_armed_turtlebot/euslisp/dxl-armed-turtlebot-interface.l") 18 | (assert (dxl-armed-turtlebot-init)) 19 | (assert (progn 20 | (send *ri* :angle-vector (send *dxl-armed-turtlebot* :angle-vector) 2000) 21 | (send *ri* :wait-interpolation) 22 | t)) 23 | (assert (progn (send *ri* :go-pos 0.1 0 5) t)) 24 | (assert (progn (send *ri* :go-velocity 0.1 0 5) t)) 25 | ) 26 | 27 | (run-all-tests) 28 | (exit 0) -------------------------------------------------------------------------------- /dxl_armed_turtlebot/test/test-dxl-armed-turtlebot.test: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/urdf/robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/worlds/model/checkerboard/checkerboard_20mm_7x4.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 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/worlds/model/checkerboard/checkerboard_20mm_7x4_color.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/dxl_armed_turtlebot/worlds/model/checkerboard/checkerboard_20mm_7x4_color.png -------------------------------------------------------------------------------- /dxl_armed_turtlebot/worlds/model/checkerboard/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | checkerboard 4 | 1.0 5 | model.sdf 6 | 7 | Moju Zhao 8 | chou@jsk.imi.i.u-tokyo.ac.jp 9 | 10 | dxl_armed_turtlebot 11 | 12 | -------------------------------------------------------------------------------- /dxl_armed_turtlebot/worlds/model/checkerboard/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 6 | 0 0 0 0 -0 0 7 | 8 | 9 | 10 | model://checkerboard/checkerboard_20mm_7x4.dae 11 | 1 1 1 12 | 13 | 14 | 15 | 16 | 17 | 100 18 | 50 19 | 20 | 21 | 22 | 0 23 | 1e+06 24 | 25 | 26 | 0 27 | 1 28 | 29 | 0 30 | 0.2 31 | 1e+13 32 | 1 33 | 0.01 34 | 0 35 | 36 | 37 | 1 38 | -0.01 39 | 0 40 | 0.2 41 | 1e+13 42 | 1 43 | 44 | 45 | 46 | 10 47 | 0 48 | 49 | 50 | 1 51 | 0 52 | 53 | 54 | model://checkerboard/checkerboard_20mm_7x4.dae 55 | 1 1 1 56 | 57 | 58 | 59 | 60 | 0 61 | 0 62 | 63 | 0 64 | 0 65 | 0 66 | 67 | 68 | 69 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(dynamixel_7dof_arm) 3 | 4 | find_package(catkin REQUIRED COMPONENTS rostest) # add roseus to gen messages 5 | 6 | catkin_package() 7 | 8 | if(CATKIN_ENABLE_TESTING) 9 | find_package(pr2eus QUIET) 10 | if(pr2eus_FOUND) 11 | add_rostest(test/test-dxl-7dof-arm.test) 12 | endif() 13 | endif() 14 | 15 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/config/dynamixel_joint_controllers.yaml: -------------------------------------------------------------------------------- 1 | arm_j1_controller: 2 | controller: 3 | package: dynamixel_controllers 4 | module: joint_position_controller 5 | type: JointPositionController 6 | joint_name: arm_joint1 7 | joint_speed: 2.0 8 | motor: 9 | id: 1 10 | init: 512 11 | min: 0 12 | max: 1023 13 | 14 | arm_j2_controller: 15 | controller: 16 | package: dynamixel_controllers 17 | module: joint_position_controller 18 | type: JointPositionController 19 | joint_name: arm_joint2 20 | joint_speed: 2.0 21 | motor: 22 | id: 2 23 | init: 512 24 | min: 0 25 | max: 1023 26 | 27 | arm_j3_controller: 28 | controller: 29 | package: dynamixel_controllers 30 | module: joint_position_controller 31 | type: JointPositionController 32 | joint_name: arm_joint3 33 | joint_speed: 2.0 34 | motor: 35 | id: 3 36 | init: 512 37 | min: 0 38 | max: 1023 39 | 40 | arm_j4_controller: 41 | controller: 42 | package: dynamixel_controllers 43 | module: joint_position_controller 44 | type: JointPositionController 45 | joint_name: arm_joint4 46 | joint_speed: 2.0 47 | motor: 48 | id: 4 49 | init: 512 50 | min: 0 51 | max: 1023 52 | 53 | arm_j5_controller: 54 | controller: 55 | package: dynamixel_controllers 56 | module: joint_position_controller 57 | type: JointPositionController 58 | joint_name: arm_joint5 59 | joint_speed: 2.0 60 | motor: 61 | id: 5 62 | init: 512 63 | min: 0 64 | max: 1023 65 | 66 | arm_j6_controller: 67 | controller: 68 | package: dynamixel_controllers 69 | module: joint_position_controller 70 | type: JointPositionController 71 | joint_name: arm_joint6 72 | joint_speed: 2.0 73 | motor: 74 | id: 6 75 | init: 512 76 | min: 0 77 | max: 1023 78 | 79 | gripper_joint_controller: 80 | controller: 81 | package: dynamixel_controllers 82 | module: joint_position_controller 83 | type: JointPositionController 84 | joint_name: gripper_joint 85 | joint_speed: 2.0 86 | motor: 87 | id: 7 88 | init: 512 89 | min: 0 90 | max: 1023 91 | 92 | fullbody_controller: 93 | controller: 94 | package: dynamixel_controllers 95 | module: joint_trajectory_action_controller 96 | type: JointTrajectoryActionController 97 | joint_trajectory_action_node: 98 | min_velocity: 0.25 99 | constraints: 100 | goal_time: 0.0 101 | 102 | gripper_controller: 103 | controller: 104 | package: dynamixel_controllers 105 | module: joint_trajectory_action_controller 106 | type: JointTrajectoryActionController 107 | joint_trajectory_action_node: 108 | min_velocity: 0.25 109 | constraints: 110 | goal_time: 0.0 111 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface.l: -------------------------------------------------------------------------------- 1 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 2 | ;; 7dof アームロボットのrobot-interfaceクラス 3 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 4 | 5 | (require :robot-interface "package://pr2eus/robot-interface.l") 6 | (require :dxl-7dof-arm-robot "package://dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l") 7 | 8 | ;; loadする台車・アームのrobot-interfaceクラスのメソッド定義ファイル 9 | (load "package://dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface-common.l") 10 | 11 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 12 | ;; 7dof アームロボットのrobot-interfaceクラス定義 13 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 14 | (defclass dxl-7dof-arm-interface 15 | :super robot-interface 16 | :slots (gripper-action)) 17 | 18 | (eval `(defmethod dxl-7dof-arm-interface 19 | ;; dxl-7dof-arm-interfaceのメソッドをdefmethodする 20 | ,@(get-method-list-for-dxl-7dof-arm-interface) 21 | )) 22 | (defmethod dxl-7dof-arm-interface 23 | (:init 24 | (&rest args) 25 | (prog1 26 | (send-super :init :robot dxl-7dof-arm-robot) 27 | (send self :initialize-arm-robot-ros) 28 | ) 29 | ) 30 | ) 31 | 32 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 33 | ;; アーム台車ロボット用初期化関数 34 | ;; robot-interface (*ri*) とモデル (*dxl-7dof-arm*)を生成する 35 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 36 | (defun dxl-7dof-arm-init () 37 | "Initialization function for *ri* and *dxl-7dof-arm*." 38 | (if (not (boundp '*ri*)) ;; real robot interface 39 | (setq *ri* (instance dxl-7dof-arm-interface :init))) 40 | (if (not (boundp '*dxl-7dof-arm*)) ;; Euslisp model 41 | (setq *dxl-7dof-arm* (instance dxl-7dof-arm-robot :init))) 42 | (objects (list *dxl-7dof-arm*)) 43 | (send *irtviewer* :change-background #f(0.9 0.9 0.9)) 44 | (send *irtviewer* :draw-objects) 45 | ) 46 | (warn ";; (dxl-7dof-arm-init) ;; for initialize ~%") 47 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/euslisp/old_examples/test-dynamixel-motor.l: -------------------------------------------------------------------------------- 1 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 2 | ;; 必要なファイルのロードとROSのtopicのsubscribe/advertise設定 3 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 4 | 5 | (ros::load-ros-manifest "dynamixel_msgs") ;; dynamixelのメッセージを用いるためのload設定 6 | (ros::roseus "test_arm") ;; ros initを行う 7 | 8 | ;; 7自由度分いっぺんに動かすサンプルになっているが、 9 | ;; いずれも1軸ずつ動かすことができる 10 | (defvar *arm-dof* 7) 11 | (setq *state-list* (make-list *arm-dof*)) 12 | 13 | ;; subscribeの設定 14 | ;; どのtopicをsubscribeするか (センサ値などの取得用) 15 | 16 | ;; dynamixel_msgs::JointState型のメッセージをsubscribeするときに呼ばれるコールバック関数 17 | ;; 具体的には/arm_j0_controllder/stateなどをsubscribeすることになる 18 | (defun dxl-state-cb 19 | (msg) 20 | (setf 21 | (elt *state-list* 22 | (if (string= (send msg :name) "gripper_joint") 23 | 6 (1- (read-from-string (string-left-trim "arm_joint" (send msg :name)))))) 24 | msg)) 25 | 26 | ;; subscriberの登録 27 | (dotimes (i *arm-dof*) 28 | (ros::subscribe 29 | (if (eq i 6) ;; subscribeしたいtopic名 30 | (format nil "/gripper_joint_controller/state") 31 | (format nil "/arm_j~d_controller/state" (1+ i))) 32 | dynamixel_msgs::JointState ;; topicの型名 33 | #'dxl-state-cb)) ;; コールバック関数の設定, #'で指定するのがEuslispのポイント 34 | 35 | ;; advertiseの設定 36 | ;; どのtopicをpublishするか (指令値などの送信用) 37 | (dotimes (i *arm-dof*) 38 | (ros::advertise 39 | (if (eq i 6) ;; topic名 40 | (format nil "/gripper_joint_controller/command") 41 | (format nil "/arm_j~d_controller/command" (1+ i))) 42 | std_msgs::Float64 ;; topicの型 43 | 1)) ;; キューの数 44 | 45 | 46 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 47 | ;; サンプル関数 48 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 49 | 50 | (defun test-read-state () ;; 各サーボの状態を全軸分読み出す 51 | (ros::spin-once) 52 | (pprint (send-all *state-list* :slots)) 53 | t) 54 | (warn ";; (test-read-state)~%") 55 | 56 | (defun test-send-angle-vector-once () ;; 各軸、一回ずつ関節を動かす 57 | (ros::spin-once) 58 | ;; 現在関節角度からそれぞれ10度動かす 59 | (let ((current-av (mapcar #'rad2deg (send-all *state-list* :current_pos)))) 60 | (dotimes (i *arm-dof*) 61 | (format t ";; send command position to arm ~d~%;; press enter~%" (1+ i)) 62 | (read-line) 63 | (ros::publish 64 | (if (eq i 6) ;; topic名 65 | (format nil "/gripper_joint_controller/command") 66 | (format nil "/arm_j~d_controller/command" (1+ i))) 67 | (instance std_msgs::Float64 :init :data (deg2rad (+ 10 (elt current-av i))))) ;; 指令値をstd_msgs::Float64に変換して送信 68 | ) 69 | t)) 70 | (warn ";; (test-send-angle-vector-once) ~%") 71 | 72 | (defun test-send-angle-vector-loop (&key (move nil)) ;; ループで各関節を動かすサンプル 73 | (ros::spin-once) 74 | (let ((current-av '(0 0 0 0 0 0 0)) 75 | (ii 0)) 76 | (do-until-key 77 | (dotimes (i *arm-dof*) 78 | (format t ";; send for arm ~d~%;; press enter~%" (1+ i)) 79 | (ros::publish 80 | (if (eq i 6) ;; topic名 81 | (format nil "/gripper_joint_controller/command") 82 | (format nil "/arm_j~d_controller/command" (1+ i))) 83 | (if move 84 | (instance std_msgs::Float64 :init :data (deg2rad (+ (* (- (mod ii 3) 1) 5) (elt current-av i)))) 85 | (instance std_msgs::Float64 :init :data (deg2rad (+ 0 (elt current-av i)))))) 86 | ) 87 | (format t ";; temps ~A~%" (send-all *state-list* :motor_temps)) 88 | (unix:usleep 1000000) ;; 1秒まつ 89 | (incf ii) 90 | ) 91 | t)) 92 | (warn ";; (test-send-angle-vector-loop :move nil) ~%") 93 | (warn ";; (test-send-angle-vector-loop :move t) ~%") 94 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/euslisp/thermo-speaker.l: -------------------------------------------------------------------------------- 1 | (ros::load-ros-manifest "dynamixel_msgs") 2 | 3 | (ros::roseus "thermo_speaker") 4 | ;;(load "package://pr2eus/speak.l") 5 | (ros::set-param "thermo_thre" 50) 6 | 7 | (defvar *arm-dof* 7) 8 | (setq *state-list* (make-list *arm-dof*)) 9 | 10 | (defun state-cb 11 | (msg) 12 | (setf 13 | (elt *state-list* 14 | (if (string= (send msg :name) "gripper_joint") 15 | 6 (1- (read-from-string (string-left-trim "arm_joint" (send msg :name)))))) 16 | msg)) 17 | 18 | (dotimes (i *arm-dof*) 19 | (ros::subscribe 20 | (if (eq i 6) 21 | (format nil "/gripper_joint_controller/state") 22 | (format nil "/arm_j~d_controller/state" (1+ i))) 23 | dynamixel_msgs::JointState 24 | #'state-cb)) 25 | 26 | (defun main () ;; [deg] 27 | (ros::rate 0.1) 28 | (do-until-key 29 | (ros::spin-once) 30 | (let ((thre (ros::get-param "thermo_thre"))) 31 | (if (and thre (every #'identity *state-list*)) 32 | (let ((mot 33 | (remove-if-not 34 | #'(lambda (x) 35 | (> (elt (send x :motor_temps) 0) thre)) 36 | *state-list*))) 37 | (when mot 38 | ;;(speak-jp "モータが/あ'つ'いです。") 39 | (warn ";; motor ~A are hot (temp ~A > ~A)~%" 40 | (mapcar #'(lambda (x) (elt x 0)) (send-all mot :motor_temps)) 41 | (mapcar #'(lambda (x) (elt x 0)) (send-all mot :motor_ids)) 42 | thre) 43 | ))) 44 | (ros::sleep) 45 | ) 46 | )) 47 | 48 | (main) 49 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/launch/dynamixel_7dof_arm_bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | namespace: dxl_manager 7 | serial_ports: 8 | 7dof_arm_port: 9 | port_name: "/dev/dynamixel_arm" 10 | baud_rate: 1000000 11 | min_motor_id: 1 12 | max_motor_id: 25 13 | update_rate: 20 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 33 | 34 | 35 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/launch/gripper_controller_spawner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 13 | 14 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/launch/soundplay.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b dynamixel_7dof_arm 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/package.xml: -------------------------------------------------------------------------------- 1 | 2 | dynamixel_7dof_arm 3 | 7.0.0 4 | 7dof dynamixel motor arm 5 | BSD 6 | Shunichi Nozawa 7 | Shunichi Nozawa 8 | 9 | catkin 10 | 11 | dynamixel_msgs 12 | dynamixel_driver 13 | dynamixel_controllers 14 | pr2eus 15 | 16 | dynamixel_msgs 17 | dynamixel_driver 18 | dynamixel_controllers 19 | pr2eus 20 | 21 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/scripts/58-dynamixel_configurator.rules: -------------------------------------------------------------------------------- 1 | # On precise, for some reason, USER and GROUP are getting ignored. 2 | # So setting mode = 0666 for now. 3 | SUBSYSTEM=="tty", ATTRS{idVendor}=="0403", ATTRS{manufacturer}=="BestTechnology", MODE:="0666", GROUP:="dialout", SYMLINK+="dynamixel_arm" 4 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/scripts/create_udev_rules: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | echo "" 4 | echo "This script copies a udev rule to /etc to facilitate bringing" 5 | echo "up the dynamixel configurator connection as /dev/ttyUSB?." 6 | echo "" 7 | 8 | sudo cp `rospack find dynamixel_7dof_arm`/scripts/58-dynamixel_configurator.rules /etc/udev/rules.d 9 | 10 | 11 | echo "" 12 | echo "Restarting udev" 13 | echo "" 14 | sudo service udev reload 15 | sudo service udev restart 16 | 17 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/scripts/gripper_controller_spawner.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | . ~/.bashrc 4 | FLAG=1; 5 | while [ $FLAG -eq 1 ]; 6 | do 7 | rostopic info /fullbody_controller/follow_joint_trajectory/result && rostopic info /fullbody_controller/follow_joint_trajectory/goal > /dev/null; 8 | if [ $? -eq 0 ]; then FLAG=0;fi; 9 | echo "Wait for fullbody_controller"; 10 | sleep 1; 11 | done 12 | echo "launch gripper_controller"; 13 | roslaunch dynamixel_7dof_arm gripper_controller_spawner.launch 14 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/scripts/reset_torque.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python 2 | 3 | import sys 4 | from optparse import OptionParser 5 | 6 | import roslib 7 | roslib.load_manifest('dynamixel_driver') 8 | 9 | from dynamixel_driver import dynamixel_io 10 | 11 | if __name__ == '__main__': 12 | usage_msg = 'Usage: %prog [options] ID [On|Off]' 13 | desc_msg = 'Turns the torque of specified Dynamixel servo motor on or off.' 14 | epi_msg = 'Example: %s --port=/dev/ttyUSB1 --baud=57600 1 Off' % sys.argv[0] 15 | 16 | parser = OptionParser(usage=usage_msg, description=desc_msg, epilog=epi_msg) 17 | parser.add_option('-p', '--port', metavar='PORT', default='/dev/ttyUSB0', 18 | help='motors of specified controllers are connected to PORT [default: %default]') 19 | parser.add_option('-b', '--baud', metavar='BAUD', type="int", default=1000000, 20 | help='connection to serial port will be established at BAUD bps [default: %default]') 21 | 22 | (options, args) = parser.parse_args(sys.argv) 23 | 24 | if len(args) < 3: 25 | parser.print_help() 26 | exit(1) 27 | 28 | port = options.port 29 | baudrate = options.baud 30 | motor_id = int(args[1]) 31 | torque_on = args[2] 32 | 33 | try: 34 | dxl_io = dynamixel_io.DynamixelIO(port, baudrate) 35 | except dynamixel_io.SerialOpenError as soe: 36 | print('ERROR:', soe) 37 | else: 38 | print('Turning torque %s for motor %d' % (torque_on, motor_id)) 39 | ret = dxl_io.ping(motor_id) 40 | if ret: 41 | from dynamixel_driver import dynamixel_const 42 | if torque_on.lower() == 'off': 43 | torque_on = False 44 | elif torque_on.lower() == 'on': 45 | if not ret[4] & dynamixel_const.DXL_OVERHEATING_ERROR == 0: 46 | dxl_io.set_torque_limit(motor_id, 1023) 47 | print("OVERHEATING -> Reset Torque") 48 | if not ret[4] & dynamixel_const.DXL_OVERLOAD_ERROR == 0: 49 | dxl_io.set_torque_limit(motor_id, 1023) 50 | print("OVERLOAD -> Reset Torque") 51 | torque_on = True 52 | else: 53 | parser.print_help() 54 | exit(1) 55 | ret = dxl_io.set_torque_enabled(motor_id, torque_on) 56 | if not ret[4] & dynamixel_const.DXL_OVERHEATING_ERROR == 0: 57 | print("OVERHEATING") 58 | exit (1) 59 | if not ret[4] & dynamixel_const.DXL_OVERLOAD_ERROR == 0: 60 | print("OVERLOAD") 61 | exit (1) 62 | print("done.") 63 | else: 64 | print('ERROR: motor %d did not respond. Make sure to specify the correct baudrate.' % motor_id) 65 | exit (1) 66 | 67 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/scripts/scan_ids.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | # -*- coding: utf-8 -*- 3 | 4 | import sys 5 | from optparse import OptionParser 6 | 7 | import roslib 8 | roslib.load_manifest('dynamixel_driver') 9 | 10 | from dynamixel_driver import dynamixel_io 11 | 12 | if __name__ == '__main__': 13 | parser = OptionParser(usage='Usage: %prog [options]', description='Changes the unique ID of a Dynamixel servo motor.') 14 | parser.add_option('-p', '--port', metavar='PORT', default='/dev/ttyUSB0', 15 | help='motors of specified controllers are connected to PORT [default: %default]') 16 | parser.add_option('-b', '--baud', metavar='BAUD', type="int", default=1000000, 17 | help='connection to serial port will be established at BAUD bps [default: %default]') 18 | parser.add_option('-f', '--from-id', metavar='FROM_ID', type="int", default=1, 19 | help='from id [default: %default]') 20 | parser.add_option('-t', '--to-id', metavar='TO_ID', type="int", default=7, 21 | help='to id [default: %default]') 22 | 23 | (options, args) = parser.parse_args(sys.argv) 24 | 25 | if len(args) < 1: 26 | parser.print_help() 27 | exit(1) 28 | 29 | port = options.port 30 | baudrate = options.baud 31 | 32 | try: 33 | dxl_io = dynamixel_io.DynamixelIO(port, baudrate) 34 | except dynamixel_io.SerialOpenError as soe: 35 | print('ERROR:', soe) 36 | else: 37 | for idx in [x + options.from_id for x in range(options.to_id - options.from_id + 1)]: 38 | print('Scanning %d...' %(idx), end=' ') 39 | if dxl_io.ping(idx): 40 | print('The motor %d respond to a ping' %(idx)) 41 | else: 42 | print('ERROR: The specified motor did not respond to id %d.' % idx) 43 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/test/test-dxl-7dof-arm.l: -------------------------------------------------------------------------------- 1 | #+:ros (require :unittest "lib/llib/unittest.l") 2 | 3 | (unless *unit-test* 4 | (init-unit-test)) 5 | 6 | (deftest test-init-model 7 | (load "package://dynamixel_7dof_arm/euslisp/dxl-7dof-arm-robot.l") 8 | (assert (setq *dxl-7dof-arm* (dxl-7dof-arm))) 9 | (assert (objects (list *dxl-7dof-arm*))) 10 | (dolist (pose (remove-if-not #'(lambda (x) (substringp "pose" (string-downcase x))) (send *dxl-7dof-arm* :methods))) 11 | (assert (send *dxl-7dof-arm* pose))) 12 | (send *dxl-7dof-arm* :tuckarm-pose) 13 | (assert (send *dxl-7dof-arm* :arm :move-end-pos (float-vector 20 20 -20) :world :rotation-axis nil)) 14 | ) 15 | 16 | (deftest test-init-ri 17 | (load "package://dynamixel_7dof_arm/euslisp/dxl-7dof-arm-interface.l") 18 | (assert (dxl-7dof-arm-init)) 19 | (assert (progn 20 | (send *ri* :angle-vector (send *dxl-7dof-arm* :angle-vector) 2000) 21 | (send *ri* :wait-interpolation) 22 | t)) 23 | ) 24 | 25 | (run-all-tests) 26 | (exit 0) -------------------------------------------------------------------------------- /dynamixel_7dof_arm/test/test-dxl-7dof-arm.test: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | -------------------------------------------------------------------------------- /dynamixel_7dof_arm/urdf/meshes/F2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/dynamixel_7dof_arm/urdf/meshes/F2.stl -------------------------------------------------------------------------------- /dynamixel_7dof_arm/urdf/meshes/ax12.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/dynamixel_7dof_arm/urdf/meshes/ax12.stl -------------------------------------------------------------------------------- /dynamixel_7dof_arm/urdf/meshes/dxl_gripper.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/dynamixel_7dof_arm/urdf/meshes/dxl_gripper.stl -------------------------------------------------------------------------------- /mechatrobot/.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | ColumnLimit: 120 4 | MaxEmptyLinesToKeep: 1 5 | SortIncludes: false 6 | 7 | Standard: Auto 8 | IndentWidth: 2 9 | TabWidth: 2 10 | UseTab: Never 11 | AccessModifierOffset: -2 12 | ConstructorInitializerIndentWidth: 2 13 | NamespaceIndentation: None 14 | ContinuationIndentWidth: 4 15 | IndentCaseLabels: true 16 | IndentFunctionDeclarationAfterType: false 17 | 18 | AlignEscapedNewlinesLeft: false 19 | AlignTrailingComments: true 20 | 21 | AllowAllParametersOfDeclarationOnNextLine: false 22 | ExperimentalAutoDetectBinPacking: false 23 | ObjCSpaceBeforeProtocolList: true 24 | Cpp11BracedListStyle: false 25 | 26 | AllowShortIfStatementsOnASingleLine: false 27 | AllowShortLoopsOnASingleLine: false 28 | AllowShortFunctionsOnASingleLine: None 29 | 30 | AlwaysBreakTemplateDeclarations: true 31 | AlwaysBreakBeforeMultilineStrings: false 32 | BreakBeforeBinaryOperators: false 33 | BreakBeforeTernaryOperators: false 34 | BreakConstructorInitializersBeforeComma: true 35 | 36 | BinPackParameters: true 37 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 38 | DerivePointerBinding: false 39 | PointerBindsToType: true 40 | 41 | PenaltyExcessCharacter: 1000 42 | PenaltyBreakBeforeFirstCallParameter: 19 43 | PenaltyBreakComment: 60 44 | PenaltyBreakFirstLessLess: 1000 45 | PenaltyBreakString: 100 46 | PenaltyReturnTypeOnItsOwnLine: 70 47 | 48 | SpacesBeforeTrailingComments: 2 49 | SpacesInParentheses: false 50 | SpacesInAngles: false 51 | SpaceInEmptyParentheses: false 52 | SpacesInCStyleCastParentheses: false 53 | SpaceAfterCStyleCast: false 54 | SpaceAfterControlStatementKeyword: true 55 | SpaceBeforeAssignmentOperators: true 56 | 57 | # Configure each individual brace in BraceWrapping 58 | BreakBeforeBraces: Custom 59 | 60 | # Control of individual brace wrapping cases 61 | BraceWrapping: 62 | AfterClass: true 63 | AfterControlStatement: true 64 | AfterEnum: true 65 | AfterFunction: true 66 | AfterNamespace: true 67 | AfterStruct: true 68 | AfterUnion: true 69 | BeforeCatch: true 70 | BeforeElse: true 71 | IndentBraces: false 72 | ... 73 | -------------------------------------------------------------------------------- /mechatrobot/.gitignore: -------------------------------------------------------------------------------- 1 | *~ -------------------------------------------------------------------------------- /mechatrobot/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package mechatrobot 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 7.0.0 (2020-11-24) 6 | ------------------ 7 | * fix invalid version number in package.xml 8 | * Merge pull request #384 from yuki-asano/mechatrobot-fix 9 | fix program 10 | * Merge branch 'master' into mechatrobot-fix 11 | * Merge pull request #383 from yuki-asano/mechatrobot 12 | update robot model 13 | * fix program 14 | * update robot model 15 | * Merge branch 'master' into arm-test 16 | * Merge branch 'master' into fix_camera_name 17 | * Merge pull request #369 from yuki-asano/mechatrobot 18 | mechatrobot for enshu 2020 19 | * Merge pull request #1 from k-okada/mechatrobot 20 | use --std=c++14 for gcc 5.7 (kinetic) 21 | * use --std=c++14 for gcc 5.7 (kinetic) to use std::literals::chrono_literals, see https://en.cppreference.com/w/cpp/symbol_index/chrono_literals 22 | * add depend 23 | * add depend 24 | * add motor command sample in python 25 | * add sampe-3d-model 26 | * add face detection sample 27 | * fix frame_id. /range -> range 28 | * fix rpm 29 | * modify rviz config 30 | * fix unit 31 | * add arduino sketch samples 32 | * fix typo 33 | * add mechatrobot pkg 34 | * Contributors: Kei Okada, Yuki Asano, ishiguroJSK 35 | 36 | 6.1.0 (2019-11-11) 37 | ------------------ 38 | 39 | 6.0.0 (2019-06-16) 40 | ------------------ 41 | 42 | 5.0.0 (2018-09-14) 43 | ------------------ 44 | 45 | 4.0.1 (2017-11-20) 46 | ------------------ 47 | 48 | 4.0.0 (2017-09-24) 49 | ------------------ 50 | 51 | 3.2.2 (2016-12-01) 52 | ------------------ 53 | 54 | 3.2.1 (2016-11-30) 55 | ------------------ 56 | 57 | 3.2.0 (2016-11-29) 58 | ------------------ 59 | 60 | 3.1.0 (2016-11-07) 61 | ------------------ 62 | 63 | 3.0.1 (2016-11-06) 64 | ------------------ 65 | 66 | 3.0.0 (2016-10-16) 67 | ------------------ 68 | 69 | 2.1.5 (2015-11-25) 70 | ------------------ 71 | 72 | 2.1.4 (2015-11-24) 73 | ------------------ 74 | 75 | 2.1.3 (2015-11-19) 76 | ------------------ 77 | 78 | 2.1.2 (2015-11-12) 79 | ------------------ 80 | 81 | 2.1.1 (2015-11-11 12:55) 82 | ------------------------ 83 | 84 | 2.1.0 (2015-11-11 03:25) 85 | ------------------------ 86 | 87 | 2.0.0 (2015-11-10) 88 | ------------------ 89 | 90 | 1.0.3 (2015-11-09) 91 | ------------------ 92 | 93 | 1.0.2 (2014-12-02) 94 | ------------------ 95 | 96 | 1.0.1 (2014-11-27) 97 | ------------------ 98 | 99 | 1.0.0 (2014-11-11) 100 | ------------------ 101 | -------------------------------------------------------------------------------- /mechatrobot/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(mechatrobot) 3 | 4 | ## https://stackoverflow.com/questions/35856969/chrono-literals-is-not-a-namespace-name 5 | if(CMAKE_COMPILER_IS_GNUCXX) 6 | execute_process(COMMAND ${CMAKE_C_COMPILER} -dumpversion OUTPUT_VARIABLE GCC_VERSION) 7 | if (GCC_VERSION VERSION_LESS 6.1) 8 | set(CMAKE_CXX_STANDARD 14) 9 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 10 | set(CMAKE_CXX_EXTENSIONS OFF) 11 | endif() 12 | endif() 13 | 14 | find_package(catkin REQUIRED COMPONENTS controller_manager cmake_modules diagnostic_updater hardware_interface std_msgs) 15 | find_package(TinyXML REQUIRED) 16 | 17 | ################################### 18 | ## catkin specific configuration ## 19 | ################################### 20 | catkin_package( 21 | DEPENDS TinyXML) 22 | 23 | ########### 24 | ## Build ## 25 | ########### 26 | 27 | include_directories(include ${catkin_INCLUDE_DIRS} ${TinyXML_INCLUDE_DIRS}) 28 | 29 | ## Declare a C++ executable for ROS_CONTROL 30 | add_executable(robot_control src/robot_control.cpp) 31 | ## Specify libraries to link a library or executable target against 32 | target_link_libraries(robot_control ${catkin_LIBRARIES} ${TinyXML_LIBRARIES}) 33 | 34 | ############# 35 | ## Install ## 36 | ############# 37 | ## Mark executables and/or libraries for installation 38 | install(TARGETS robot_control 39 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 40 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 41 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 42 | ) 43 | 44 | install(DIRECTORY config launch sketchbook urdf 45 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 46 | ) 47 | 48 | ############# 49 | ## Testing ## 50 | ############# 51 | if(CATKIN_ENABLE_TESTING) 52 | find_package(catkin REQUIRED COMPONENTS roslaunch roslint) 53 | file(GLOB LAUNCH_FILES launch/*.launch) 54 | foreach(LAUNCH_FILE ${LAUNCH_FILES}) 55 | roslaunch_add_file_check(${LAUNCH_FILE}) 56 | endforeach() 57 | 58 | file(GLOB CPP_FILES src/*.cpp) 59 | message("test ${CPP_FILES}") 60 | roslint_cpp(${CPP_FILES}) 61 | endif() 62 | -------------------------------------------------------------------------------- /mechatrobot/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | joint_state_controller: 2 | type: joint_state_controller/JointStateController 3 | publish_rate: 50 4 | 5 | # Position - Joint Position Trajectory Controllers ------------------- 6 | position_trajectory_controller: 7 | type: "position_controllers/JointTrajectoryController" 8 | joints: 9 | - joint1 10 | constraints: 11 | goal_time: 2.0 # Defaults to zero 12 | stopped_velocity_tolerance: 0.1 # Defaults to 0.01 13 | joint1: 14 | trajectory: 0.0 15 | goal: 0.2 16 | 17 | state_publish_rate: 50 # Defaults to 50 18 | action_monitor_rate: 20 # Defaults to 20 19 | #hold_trajectory_duration: 0 # Defaults to 0.5 20 | -------------------------------------------------------------------------------- /mechatrobot/euslisp/motor-command-by-face.l: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env roseus 2 | 3 | ;; 原点->x 4 | ;; | 5 | ;; y 6 | ;;------------------- 7 | ;;| | 8 | ;;| | 9 | ;;| | 10 | ;;| | 11 | ;;------------------- 12 | 13 | (ros::load-ros-manifest "roseus") 14 | (ros::load-ros-manifest "opencv_apps") 15 | (ros::roseus "motor-command-by-face") 16 | 17 | (ros::advertise "motor1/command" std_msgs::Int64 1) 18 | (ros::rate 10) 19 | 20 | (setq *image-size* (float-vector 640 480)) ;;pixel 21 | (setq *image-center* (scale 0.5 *image-size*)) 22 | (setq *face-pos* (float-vector 0.0 0.0)) 23 | (setq *motor-angle* 0) ;; [deg] 24 | 25 | (defun face-detection-cb (msg) 26 | (let ((motor-command-msg (instance std_msgs::Int64 :init)) 27 | face) 28 | ;; face check 29 | (if (send msg :faces) 30 | (progn 31 | (setq face (send (elt (send msg :faces) 0) :face)) 32 | (setq *face-pos* (float-vector (send face :x) (send face :y))) 33 | ;;顔がカメラの左右どちらかをチェック 34 | (if (<= (elt *face-pos* 0) (elt *image-center* 0)) 35 | (setq *motor-angle* (- *motor-angle* 5)) 36 | (setq *motor-angle* (+ *motor-angle* 5))) 37 | ;; 38 | (send motor-command-msg :data *motor-angle*) 39 | ;; print 40 | (format t "face-pos(x, y): (~a, ~a)~%" 41 | (elt *face-pos* 0) (elt *face-pos* 1)) 42 | (format t "/motor1/command ~a~%~%" 43 | (send motor-command-msg :data)) 44 | ;; publish 45 | (ros::publish "/motor1/command" motor-command-msg) 46 | ) 47 | (progn 48 | (warn "no faces~%"))) 49 | )) 50 | 51 | (ros::subscribe "face_detection/faces" opencv_apps::FaceArrayStamped #'face-detection-cb) 52 | 53 | 54 | (warn "(main)~%") 55 | (defun main () 56 | (do-until-key 57 | (ros::spin-once) 58 | (ros::sleep) 59 | )) 60 | 61 | (main) 62 | -------------------------------------------------------------------------------- /mechatrobot/launch/mechatrobot_controller.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 8 | 9 | 10 | 11 | 12 | 13 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 25 | 29 | 30 | 31 | 33 | 34 | -------------------------------------------------------------------------------- /mechatrobot/launch/mechatrobot_display.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 5 | 6 | 7 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /mechatrobot/launch/mechatrobot_driver.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 7 | 8 | 9 | -------------------------------------------------------------------------------- /mechatrobot/launch/sample_face_detect.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | -------------------------------------------------------------------------------- /mechatrobot/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | mechatrobot 4 | 7.0.0 5 | The mechatrobot package 6 | 7 | Kei Okada 8 | Yuki Asano 9 | Kei Okada 10 | Yuki Asano 11 | 12 | BSD 13 | 14 | catkin 15 | 16 | controller_manager 17 | cmake_modules 18 | diagnostic_updater 19 | hardware_interface 20 | rosserial_arduino 21 | std_msgs 22 | tinyxml 23 | 24 | controller_manager 25 | diagnostic_updater 26 | hardware_interface 27 | opencv_apps 28 | robot_state_publisher 29 | rosserial_arduino 30 | rosserial_client 31 | rosserial_python 32 | rqt_joint_trajectory_controller 33 | rviz 34 | sensor_msgs 35 | std_msgs 36 | tf 37 | tinyxml 38 | usb_cam 39 | 40 | roslaunch 41 | roslint 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /mechatrobot/scripts/motor-command-by-face.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python3 2 | 3 | import rospy 4 | from opencv_apps.msg import FaceArrayStamped 5 | from std_msgs.msg import Int64 6 | 7 | image_size = [640, 480] # pixel 8 | image_center = list([x/2 for x in image_size]) 9 | motor_angle = 0 # [deg] 10 | 11 | def face_detection_cb(msg): 12 | pub = rospy.Publisher('motor1/command', Int64, queue_size=1) 13 | 14 | motor_command_msg = Int64() 15 | face_pos = [0, 0] 16 | global motor_angle 17 | 18 | # face check 19 | if len(msg.faces): 20 | face = msg.faces[0].face 21 | face_pos[0] = face.x 22 | face_pos[1] = face.y 23 | # check face position. left or right 24 | if face_pos[0] <= image_center[0]: 25 | motor_angle -= 5 26 | else: 27 | motor_angle += 5 28 | 29 | motor_command_msg.data = motor_angle 30 | 31 | # print 32 | print("face_pos(x, y): ({} {})".format(face_pos[0], face_pos[1])) 33 | print("/motor1/command: {}\n".format(motor_command_msg.data)) 34 | 35 | # publish 36 | pub.publish(motor_command_msg) 37 | else: 38 | print("no faces") 39 | 40 | 41 | def main(): 42 | rospy.init_node('motor_command_by_face', anonymous=True) 43 | rospy.Subscriber('face_detection/faces', FaceArrayStamped, face_detection_cb) 44 | rate = rospy.Rate(10) 45 | 46 | rospy.spin() 47 | rate.sleep() 48 | 49 | if __name__ == '__main__': 50 | main() 51 | 52 | -------------------------------------------------------------------------------- /mechatrobot/sketchbook/dynamixel_motor_sample/dynamixel_motor_sample.ino: -------------------------------------------------------------------------------- 1 | #include "src/DynamixelAX12A.h" 2 | 3 | #define BAUDRATE 115200 4 | #define LED_PIN 13 5 | 6 | void setup() 7 | { 8 | dynamixel_setup(); 9 | 10 | Serial.begin(BAUDRATE); 11 | 12 | pinMode(LED_PIN, OUTPUT); 13 | } 14 | 15 | float angle; 16 | int servo_id = 7; 17 | 18 | void write_test(){ 19 | goal_position(servo_id, 150); 20 | Serial.println("move to 150 deg"); 21 | delay(1000); 22 | 23 | goal_position(servo_id, 120); 24 | Serial.println("move to 120 deg"); 25 | delay(1000); 26 | } 27 | 28 | void read_test(){ 29 | angle = read_position(servo_id); 30 | if (angle < 150){ 31 | digitalWrite(LED_PIN, HIGH); 32 | } else { 33 | digitalWrite(LED_PIN, LOW); 34 | } 35 | Serial.println(angle, DEC); 36 | delay(1000); 37 | } 38 | 39 | void read_write_test(){ 40 | goal_position(servo_id, 150); 41 | Serial.println("goal:150 deg"); 42 | delay(1000); 43 | angle = read_position(servo_id); 44 | Serial.print("current:"); 45 | Serial.println(angle); 46 | 47 | goal_position(servo_id, 120);; 48 | Serial.println("goal:120 deg"); 49 | delay(1000); 50 | angle = read_position(servo_id); 51 | Serial.print("current:"); 52 | Serial.println(angle); 53 | } 54 | 55 | void loop() 56 | { 57 | /* write_test(); */ 58 | read_test(); 59 | /* read_write_test(); */ 60 | } 61 | -------------------------------------------------------------------------------- /mechatrobot/sketchbook/led_sample/led_sample.ino: -------------------------------------------------------------------------------- 1 | void setup() { 2 | pinMode(2, OUTPUT); // digital pinのモードを設定 3 | } 4 | 5 | void loop() { 6 | 7 | digitalWrite(2, HIGH); // digital pinをHIGH/LOWに切り替え 8 | delay(1000); // sleep [msec] 9 | digitalWrite(2, LOW); 10 | delay(1000); 11 | 12 | } 13 | -------------------------------------------------------------------------------- /mechatrobot/sketchbook/ros_dynamixel_motor_sample/ros_dynamixel_motor_sample.ino: -------------------------------------------------------------------------------- 1 | /////////////////////////////////////////////////////////////////////////////// 2 | // Copyright (C) 2021, Kei Okada and Kunio Kojima 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 | // * Redistributions of source code must retain the above copyright notice, 7 | // this list of conditions and the following disclaimer. 8 | // * Redistributions in binary form must reproduce the above copyright 9 | // notice, this list of conditions and the following disclaimer in the 10 | // documentation and/or other materials provided with the distribution. 11 | // * Neither the name of Kei Okada nor the names of its 12 | // contributors may be used to endorse or promote products derived from 13 | // this software without specific prior written permission. 14 | // 15 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 | // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 19 | // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 | // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 | // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 | // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 | // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 | // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 | // POSSIBILITY OF SUCH DAMAGE. 26 | ////////////////////////////////////////////////////////////////////////////// 27 | 28 | #define BAUDRATE 115200 29 | 30 | // Dynamixel 31 | #include "src/DynamixelAX12A.h" 32 | 33 | float angle; 34 | int servo_id = 7; 35 | 36 | // ROS 37 | #include 38 | ros::NodeHandle nh; 39 | #include 40 | 41 | // publisher 42 | std_msgs::Float32 angle_msg; 43 | ros::Publisher angle_position_sub("angle/position", &angle_msg); 44 | 45 | // subscriber 46 | void angle_cb(const std_msgs::Float32& msg) 47 | { 48 | // ROSシリアルを通じて目標角度をsubscribeするとサーボに角度指令を送る 49 | goal_position(servo_id, msg.data); 50 | } 51 | ros::Subscriber angle_command_sub("angle/command", angle_cb); 52 | std_msgs::Float32 angle_command_msgs; 53 | 54 | void setup() 55 | { 56 | dynamixel_setup(); 57 | 58 | Serial.begin(BAUDRATE); // serial port settings 59 | 60 | // setup ros functions 61 | nh.initNode(); 62 | angle_msg.data = 0; 63 | nh.advertise(angle_position_sub); 64 | nh.subscribe(angle_command_sub); 65 | } 66 | 67 | void loop() 68 | { 69 | // サーボの関節角度を読みだしてROSシリアルでpublishする 70 | angle = read_position(servo_id); 71 | angle_msg.data = angle; 72 | angle_position_sub.publish(&angle_msg); 73 | 74 | nh.spinOnce(); 75 | } 76 | -------------------------------------------------------------------------------- /mechatrobot/sketchbook/ros_dynamixel_motor_sample/src: -------------------------------------------------------------------------------- 1 | ../dynamixel_motor_sample/src/ -------------------------------------------------------------------------------- /mechatrobot/sketchbook/stepping_motor_sample_28BYJ48/stepping_motor_sample_28BYJ48.ino: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #define BAUD 9600 // シリアル通信のボーレート 4 | #define MOTOR_PIN1 5 // 使用するモータのpin 5 | #define MOTOR_PIN2 6 6 | #define MOTOR_PIN3 7 7 | #define MOTOR_PIN4 8 8 | #define STEPS_PER_ROTATE_28BYJ48 2048 // 1回転に必要なステップ数. 360[deg] / 5.625[deg/step] / 2(相励磁) * 64(gear比) 9 | 10 | const int StepsPerRotate = STEPS_PER_ROTATE_28BYJ48; 11 | 12 | // 毎分の回転数(rpm) 13 | int rpm = 5; // 1-15rpmでないと動かない 14 | 15 | // モータに与えるステップ数 16 | int Steps = 512; // 90度回転. 360deg : 90deg = 2048 : 512 17 | 18 | // ライブラリとモータ配線の整合性を取り, C1, C2を入れ替える 19 | // ref https://github.com/arduino-libraries/Stepper/blob/master/src/Stepper.cpp 20 | Stepper myStepper(StepsPerRotate, MOTOR_PIN1, MOTOR_PIN3, MOTOR_PIN2, MOTOR_PIN4); 21 | 22 | void setup() { 23 | Serial.begin(BAUD); // シリアル通信の初期化 24 | myStepper.setSpeed(rpm); // rpmを設定 25 | } 26 | 27 | void loop() { 28 | 29 | // ステッピングモータを正転 30 | Serial.println("Forward"); 31 | myStepper.step(Steps); 32 | delay(500); 33 | 34 | Serial.println(); 35 | 36 | } 37 | -------------------------------------------------------------------------------- /mechatrobot/sketchbook/ultrasonic_sensor_sample/ultrasonic_sensor_sample.ino: -------------------------------------------------------------------------------- 1 | // HC-SR04 Ultrasonic sensor 2 | // https://create.arduino.cc/projecthub/Isaac100/getting-started-with-the-hc-sr04-ultrasonic-sensor-036380 3 | 4 | #define TRIG_PIN 9 5 | #define ECHO_PIN 10 6 | 7 | float duration, distance; 8 | 9 | void setup() { 10 | pinMode(TRIG_PIN, OUTPUT); 11 | pinMode(ECHO_PIN, INPUT); 12 | Serial.begin(9600); 13 | } 14 | 15 | void loop() { 16 | 17 | // calculate distance 18 | digitalWrite(TRIG_PIN, LOW); 19 | delayMicroseconds(2); 20 | digitalWrite(TRIG_PIN, HIGH); 21 | delayMicroseconds(10); 22 | digitalWrite(TRIG_PIN, LOW); 23 | 24 | duration = pulseIn(ECHO_PIN, HIGH); 25 | if(duration>0) { 26 | distance = (duration*.0343)/2; // ultrasonic speed is 340m/s = 0.034cm/us 27 | Serial.print(duration); 28 | Serial.print(" us "); 29 | Serial.print(distance); 30 | Serial.println(" cm"); 31 | } 32 | delay(200); 33 | 34 | if(distance > 6) { 35 | Serial.println("outside"); 36 | delay(100); 37 | } else if (distance <= 6) { 38 | Serial.println("inside"); 39 | delay(100); 40 | } 41 | 42 | Serial.println(""); 43 | } 44 | -------------------------------------------------------------------------------- /mechatrobot/urdf/28BYJ-48.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/mechatrobot/urdf/28BYJ-48.STL -------------------------------------------------------------------------------- /mechatrobot/urdf/HC-SR04.STL: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/mechatrobot/urdf/HC-SR04.STL -------------------------------------------------------------------------------- /mechatrobot/urdf/robot.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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /mechatrobot/urdf/sample-3d-model.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jsk-enshu/robot-programming/076c0057d8f86104bd11a7c2fe458219248d2bd7/mechatrobot/urdf/sample-3d-model.stl -------------------------------------------------------------------------------- /turtleboteus/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package turtleboteus 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 6.1.0 (2019-11-11) 6 | ------------------ 7 | 8 | 6.0.0 (2019-06-16) 9 | ------------------ 10 | * run test/*.test only when pr2eus installed (`#272 `_) 11 | * Contributors: Kei Okada 12 | 13 | 5.0.0 (2018-09-14) 14 | ------------------ 15 | 16 | 4.0.1 (2017-11-20) 17 | ------------------ 18 | 19 | 4.0.0 (2017-09-24) 20 | ------------------ 21 | * enable to run travis on kinetic (`#171 `_) 22 | * smart_battery_msgs is obsolete in kinetic, use sensor_msgs/BatteryState 23 | * Contributors: Yuki Asano 24 | 25 | 3.2.2 (2016-12-01) 26 | ------------------ 27 | 28 | 3.2.1 (2016-11-30) 29 | ------------------ 30 | 31 | 3.2.0 (2016-11-29) 32 | ------------------ 33 | 34 | 3.1.0 (2016-11-07) 35 | ------------------ 36 | 37 | 3.0.1 (2016-11-06) 38 | ------------------ 39 | 40 | 3.0.0 (2016-10-16) 41 | ------------------ 42 | 43 | 2.1.5 (2015-11-25) 44 | ------------------ 45 | 46 | 2.1.4 (2015-11-24) 47 | ------------------ 48 | 49 | 2.1.3 (2015-11-19) 50 | ------------------ 51 | 52 | 2.1.2 (2015-11-12) 53 | ------------------ 54 | 55 | 2.1.1 (2015-11-11) 56 | ------------------ 57 | 58 | 2.1.0 (2015-11-11) 59 | ------------------ 60 | 61 | 2.0.0 (2015-11-10) 62 | ------------------ 63 | * linux_hardware is obsoulte, use linux_peripheral_interface or smart_battery_msgs 64 | * ;; turtlebot-interface-common.l; I'm not sure but without this change, the knaji code crashes 65 | * dxl_armed_turtlebot, dynamixel_7dof_arm, turtleboteus: add roseus tofind_package() to gen messages 66 | * {daisya_euslisp_tutorials,turtleboteus}/{package.xml,CMakeLists.txt}: add rostest to {build,run}_depend 67 | * turtleboteus/package.xml: linux_hardware is now move to linux_peripheral_interface and smart_battery_msgs 68 | * {turtleboteus,dynamixel_7dof_arm}/package.xml roseus_msgs is not longer required 69 | * Contributors: Kei Okada 70 | 71 | 1.0.3 (2015-11-09) 72 | ------------------ 73 | 74 | 1.0.2 (2014-12-01) 75 | ------------------ 76 | * Add go-pos methods documentation 77 | * Use require instead of load 78 | * Eval generated defmethod outside of :init 79 | * Contributors: Shunichi Nozawa 80 | 81 | 1.0.1 (2014-11-27) 82 | ------------------ 83 | * Fix view-up orientation, which is reported in https://github.com/jsk-enshu/robot-programming/issues/23 84 | * Move simulation check to turtlebot-interface.l 85 | * Fix cameras and bumper-sensors accessors 86 | * Add documentation for Euslisp codes 87 | * Add turtleboteus rostest and update other tests 88 | * Add turtleboteus package and use it from dxl-armed-turtlebot 89 | * Contributors: Shunichi Nozawa 90 | -------------------------------------------------------------------------------- /turtleboteus/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(turtleboteus) 3 | 4 | find_package(catkin REQUIRED kobuki_msgs control_msgs move_base_msgs roseus rostest) # add roseus to gen messages 5 | 6 | catkin_package() 7 | 8 | if(CATKIN_ENABLE_TESTING) 9 | find_package(pr2eus QUIET) 10 | if(pr2eus_FOUND) 11 | add_rostest(test/test-turtlebot.test) 12 | endif() 13 | endif() 14 | 15 | -------------------------------------------------------------------------------- /turtleboteus/euslisp/turtlebot-interface.l: -------------------------------------------------------------------------------- 1 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 2 | ;; 台車ロボットのrobot-interfaceクラス 3 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 4 | 5 | (require :robot-interface "package://pr2eus/robot-interface.l") 6 | (require :turtlebot-with-sensors-robot "package://turtleboteus/euslisp/turtlebot-with-sensors-robot.l") 7 | 8 | ;; loadする台車のrobot-interfaceクラスのメソッド定義ファイル 9 | (require :turtlebot-interface-common "package://turtleboteus/euslisp/turtlebot-interface-common.l") 10 | 11 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 12 | ;; 台車ロボットのrobot-interfaceクラス定義 13 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 14 | (defclass turtlebot-interface 15 | :super robot-interface 16 | :slots ()) 17 | 18 | (eval `(defmethod turtlebot-interface 19 | ,@(get-method-list-for-turtlebot-interface) 20 | )) 21 | (defmethod turtlebot-interface 22 | (:init 23 | (&rest args) 24 | (prog1 25 | (send-super* :init :robot turtlebot-with-sensors-robot args) 26 | (send self :initialize-turtlebot-ros) 27 | ) 28 | ) 29 | ;; TODO : this should be merged pr2 base controller 30 | ;; https://github.com/jsk-ros-pkg/jsk_common/issues/254 31 | (:add-controller 32 | (&rest args) 33 | (prog1 34 | (send-super* :add-controller args) 35 | (if (and (send self :simulation-modep) 36 | (ros::get-param "/turtlebot_laptop_battery/acpi_path")) ;; check if real robot 37 | (setq joint-action-enable t)) 38 | )) 39 | ) 40 | 41 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 42 | ;; アーム台車ロボット用初期化関数 43 | ;; robot-interface (*ri*) とモデル (*turtlebot*)を生成する 44 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 45 | (defun turtlebot-init (&key (objects)) 46 | "Initialization function for *ri* and *turtlebot*." 47 | (if (not (boundp '*ri*)) 48 | (setq *ri* (instance turtlebot-interface :init :objects objects))) 49 | (if (not (boundp '*turtlebot*)) 50 | (setq *turtlebot* (turtlebot-with-sensors))) 51 | (objects (list *turtlebot*)) 52 | (send *irtviewer* :change-background #f(0.9 0.9 0.9)) 53 | (send *irtviewer* :draw-objects) 54 | ) 55 | (warn ";; (turtlebot-init) ;; for initialize ~%") -------------------------------------------------------------------------------- /turtleboteus/euslisp/turtlebot-with-sensors-robot.l: -------------------------------------------------------------------------------- 1 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 2 | ;; turtlebotの台車ロボット 3 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 4 | 5 | ;; turtlebotのロボットモデルクラス (urdfから自動変換して生成) 6 | (require "package://turtleboteus/models/kobuki_hexagons_kinect.l") 7 | 8 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 9 | ;; 台車ロボットモデルクラス 10 | ;; Euslispでセンサ等を追加 11 | ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 12 | (defclass turtlebot-with-sensors-robot 13 | :super turtlebot-robot 14 | :slots (sensors bumper-sensors) 15 | ) 16 | 17 | (defmethod turtlebot-with-sensors-robot 18 | (:init 19 | (&rest args 20 | &key (name "turtlebot-with-sensors-robot")) 21 | (prog1 22 | (send-super* :init :name name args) 23 | ;; add sensor 24 | (let ((bumper-links 25 | ;; bumper is named as "cliff" 26 | (remove-if-not #'(lambda (x) (let ((nm (string-downcase (car x)))) (and (substringp "link" nm) (substringp "cliff" nm)))) 27 | (send self :slots)))) 28 | (dolist (b bumper-links) 29 | (let ((bb (make-cylinder 25 50))) 30 | (send bb :newcoords (send (cdr b) :copy-worldcoords)) 31 | (send bb :rotate -pi/2 :y) 32 | (push (instance bumper-model :init bb :name (read-from-string (format nil ":~A" (string-right-trim "_" (string-right-trim "link" (send (cdr b) :name)))))) bumper-sensors) 33 | ))) 34 | (let ((acamera (send self :camera_rgb_optical_frame_lk))) 35 | (let ((bb (make-cube 10 10 10))) 36 | (send bb :newcoords (send acamera :copy-worldcoords)) 37 | (push (instance camera-model :init bb :name :camera_rgb_optical :view-up (send bb :rotate-vector (float-vector 0 1 0))) cameras) 38 | )) 39 | (let ((acamera (send self :camera_depth_frame_lk))) 40 | (let ((bb (make-cube 10 10 10))) 41 | (send bb :newcoords (send acamera :copy-worldcoords)) 42 | (push (instance camera-model :init bb :name :camera_depth) cameras) 43 | )) 44 | (setq sensors (append cameras bumper-sensors)) 45 | (dolist (s sensors) 46 | (eval `(defmethod ,(send (class self) :name) 47 | (,(send s :name) (&rest args) ,s))) 48 | (send self :assoc s)) 49 | t)) 50 | (:simulate 51 | (objs) 52 | (send-all sensors :simulate 53 | (flatten (mapcar #'(lambda (obj) (if (find-method obj :bodies) (send obj :bodies) obj)) objs)))) 54 | ;; sensor accessor 55 | (:bumper-sensors () "Returns bumper sensors." bumper-sensors) 56 | (:bumper-sensor (sensor-name) "Returns bumper sensor of given name" (send self :get-sensor-method :bumper-sensor sensor-name)) 57 | ) 58 | 59 | (defun turtlebot-with-sensors 60 | () 61 | "Generation function for turtlebot-with-sensors-robot." 62 | (instance turtlebot-with-sensors-robot :init)) -------------------------------------------------------------------------------- /turtleboteus/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | turtleboteus 4 | 7.0.0 5 | The turtleboteus package 6 | BSD 7 | Shunichi Nozawa 8 | Shunichi Nozawa 9 | catkin 10 | 11 | kobuki_msgs 12 | control_msgs 13 | move_base_msgs 14 | pr2eus 15 | rostest 16 | 17 | kobuki_msgs 18 | control_msgs 19 | move_base_msgs 20 | pr2eus 21 | rostest 22 | 23 | -------------------------------------------------------------------------------- /turtleboteus/test/test-turtlebot.l: -------------------------------------------------------------------------------- 1 | #+:ros (require :unittest "lib/llib/unittest.l") 2 | 3 | (unless *unit-test* 4 | (init-unit-test)) 5 | 6 | (deftest test-init-model 7 | (load "package://turtleboteus/euslisp/turtlebot-with-sensors-robot.l") 8 | (assert (setq *turtlebot* (turtlebot-with-sensors))) 9 | (assert (objects (list *turtlebot*))) 10 | ) 11 | 12 | (deftest test-init-ri 13 | (load "package://turtleboteus/euslisp/turtlebot-interface.l") 14 | (assert (turtlebot-init)) 15 | (assert (progn (send *ri* :go-pos 0.1 0 5) t)) 16 | (assert (progn (send *ri* :go-velocity 0.1 0 5) t)) 17 | ) 18 | 19 | (run-all-tests) 20 | (exit 0) -------------------------------------------------------------------------------- /turtleboteus/test/test-turtlebot.test: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | --------------------------------------------------------------------------------