diff --git a/nextage_tutorials/CMakeLists.txt b/nextage_tutorials/CMakeLists.txt new file mode 100644 index 00000000..0a8d8182 --- /dev/null +++ b/nextage_tutorials/CMakeLists.txt @@ -0,0 +1,50 @@ +cmake_minimum_required(VERSION 2.8.3) +project(nextage_tutorials) + +find_package(catkin REQUIRED + nextage_description) + +catkin_package() + +if(EXISTS ${nextage_description_PREFIX}/share/nextage_description/urdf/NextageOpen.urdf) + set(nextage_urdf ${nextage_description_PREFIX}/share/nextage_description/urdf/NextageOpen.urdf) +elseif(EXISTS ${nextage_description_SOURCE_PREFIX}/urdf/NextageOpen.urdf) + set(nextage_urdf ${nextage_description_SOURCE_PREFIX}/urdf/NextageOpen.urdf) +else() + message(WARNING "Could not found nextage.urdf in ${nextage_description_PREFIX}/share/nextage_description/urdf/NextageOpen.urdf nor ${nextage_description_SOURCE_PREFIX}/urdf/NextageOpen.urdf") +endif() +if (EXISTS ${nextage_urdf}) + message(STATUS "Found nextage.urdf at ${nextage_urdf}") + add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/nextage.l + COMMAND rosrun euscollada collada2eus nextage.dae config/nextage.yaml nextage.l + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + DEPENDS nextage.dae config/nextage.yaml) + add_custom_command(OUTPUT ${PROJECT_SOURCE_DIR}/nextage.dae + COMMAND rosrun collada_urdf urdf_to_collada ${nextage_urdf} nextage.dae + WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} + DEPENDS ${nextage_urdf}) + + # get collada-dom version + find_package(PkgConfig) + pkg_check_modules(COLLADA collada-dom>=2.4.4 ) + if ( ${COLLADA_FOUND} ) + add_custom_target(generate_nextage_lisp ALL DEPENDS ${PROJECT_SOURCE_DIR}/nextage.l) + else() + pkg_check_modules(COLLADA collada-dom) + message(WARNING "urdf_to_collada requries collada-dom >= 2.4.4, installed version is ${COLLADA_VERSION}") + endif() +endif() + +############# +## Install ## +############# + +install(DIRECTORY config euslisp launch model sample + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + USE_SOURCE_PERMISSIONS) + +install(FILES nextage.dae nextage.l DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) + +############ +## Test ## +############ diff --git a/nextage_tutorials/config/nextage.rviz b/nextage_tutorials/config/nextage.rviz new file mode 100644 index 00000000..efa36bb8 --- /dev/null +++ b/nextage_tutorials/config/nextage.rviz @@ -0,0 +1,361 @@ +Panels: + - Class: rviz/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /PointCloud21 + Splitter Ratio: 0.3467600643634796 + Tree Height: 235 + - Class: rviz/Selection + Name: Selection + - Class: rviz/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /2D Nav Goal1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Preferences: + PromptSaveOnExit: true +Toolbars: + toolButtonStyle: 2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz/RobotModel + Collision Enabled: false + Enabled: true + Links: + All Links Enabled: true + CHEST_JOINT0_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + HEAD_JOINT0_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + HEAD_JOINT1_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT0_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT1_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT2_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT3_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT4_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LARM_JOINT5_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RARM_JOINT0_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT1_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT2_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT3_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT4_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RARM_JOINT5_Link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + WAIST: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + head_camera_depth_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_depth_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_rgb_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + head_camera_rgb_optical_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + odom: + Alpha: 1 + Show Axes: false + Show Trail: false + Name: RobotModel + Robot Description: robot_description + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + CHEST_JOINT0_Link: + Value: true + HEAD_JOINT0_Link: + Value: true + HEAD_JOINT1_Link: + Value: true + LARM_JOINT0_Link: + Value: true + LARM_JOINT1_Link: + Value: true + LARM_JOINT2_Link: + Value: true + LARM_JOINT3_Link: + Value: true + LARM_JOINT4_Link: + Value: true + LARM_JOINT5_Link: + Value: true + RARM_JOINT0_Link: + Value: true + RARM_JOINT1_Link: + Value: true + RARM_JOINT2_Link: + Value: true + RARM_JOINT3_Link: + Value: true + RARM_JOINT4_Link: + Value: true + RARM_JOINT5_Link: + Value: true + WAIST: + Value: true + head_camera_depth_frame: + Value: true + head_camera_depth_optical_frame: + Value: true + head_camera_rgb_frame: + Value: true + head_camera_rgb_optical_frame: + Value: true + odom: + Value: true + Marker Scale: 0.4000000059604645 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + odom: + WAIST: + CHEST_JOINT0_Link: + HEAD_JOINT0_Link: + HEAD_JOINT1_Link: + head_camera_depth_frame: + head_camera_depth_optical_frame: + {} + head_camera_rgb_frame: + head_camera_rgb_optical_frame: + {} + LARM_JOINT0_Link: + LARM_JOINT1_Link: + LARM_JOINT2_Link: + LARM_JOINT3_Link: + LARM_JOINT4_Link: + LARM_JOINT5_Link: + {} + RARM_JOINT0_Link: + RARM_JOINT1_Link: + RARM_JOINT2_Link: + RARM_JOINT3_Link: + RARM_JOINT4_Link: + RARM_JOINT5_Link: + {} + Update Interval: 0 + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 0.8350027203559875 + Min Value: 2.86102294921875e-6 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Queue Size: 10 + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: /head_camera/depth_registered/points + Unreliable: false + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz/Image + Enabled: true + Image Topic: /head_camera/rgb/image_raw + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: Image + Normalize Range: true + Queue Size: 2 + Transport Hint: raw + Unreliable: false + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Default Light: true + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz/Interact + Hide Inactive Objects: true + - Class: rviz/MoveCamera + - Class: rviz/Select + - Class: rviz/FocusCamera + - Class: rviz/Measure + - Class: rviz/SetInitialPose + Theta std deviation: 0.2617993950843811 + Topic: /initialpose + X std deviation: 0.5 + Y std deviation: 0.5 + - Class: rviz/SetGoal + Topic: /move_base_simple/goal + - Class: rviz/PublishPoint + Single click: true + Topic: /clicked_point + Value: true + Views: + Current: + Class: rviz/Orbit + Distance: 2.6546847820281982 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0.11585560441017151 + Y: 0.3534391522407532 + Z: 0.8110464811325073 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.3753983974456787 + Target Frame: + Value: Orbit (rviz) + Yaw: 5.338571548461914 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 683 + Hide Left Dock: false + Hide Right Dock: true + Image: + collapsed: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000020dfc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d00000176000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006501000001b9000000910000001600ffffff000000010000010f00000319fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000319000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004610000003efc0100000002fb0000000800540069006d0065010000000000000461000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000003050000020d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1121 + X: 185 + Y: 175 diff --git a/nextage_tutorials/config/nextage.yaml b/nextage_tutorials/config/nextage.yaml new file mode 100644 index 00000000..650f16e9 --- /dev/null +++ b/nextage_tutorials/config/nextage.yaml @@ -0,0 +1,38 @@ +## +## - collada_joint_name : euslisp_joint_name (start with :) +## + +torso: + - CHEST_JOINT0 : torso-waist-y +rarm: + - RARM_JOINT0 : rarm-shoulder-y + - RARM_JOINT1 : rarm-shoulder-p + - RARM_JOINT2 : rarm-shoulder-r + - RARM_JOINT3 : rarm-elbow-p + - RARM_JOINT4 : rarm-elbow-r + - RARM_JOINT5 : rarm-wrist-p +larm: + - LARM_JOINT0 : larm-shoulder-y + - LARM_JOINT1 : larm-shoulder-p + - LARM_JOINT2 : larm-shoulder-r + - LARM_JOINT3 : larm-elbow-p + - LARM_JOINT4 : larm-elbow-r + - LARM_JOINT5 : larm-wrist-p +head: + - HEAD_JOINT0 : head-neck-y + - HEAD_JOINT1 : head-neck-p + +angle-vector: + reset-pose : [0, -0.6, 0, -100, 15.2, 9.4, 3.2, .6, 0, -100, -15.2, 9.4, -3.2, 0, 0] + +rarm-end-coords: + parent : RARM_JOINT5_Link + translate : [-0.19, 0, 0] + rotate : [0, 1, 0, 90] +larm-end-coords: + parent : LARM_JOINT5_Link + translate : [-0.19, 0, 0] + rotate : [0, 1, 0, 90] +head-end-coords: + translate : [0.06, 0, 0.025] + rotate : [0, 1, 0, 90] diff --git a/nextage_tutorials/euslisp/nextage-interface.l b/nextage_tutorials/euslisp/nextage-interface.l new file mode 100644 index 00000000..6c6e55ed --- /dev/null +++ b/nextage_tutorials/euslisp/nextage-interface.l @@ -0,0 +1,50 @@ +(load "package://pr2eus/robot-interface.l") +(require :nextage "package://nextage_tutorials/nextage.l") + +(defclass nextage-interface + :super robot-interface + :slots ()) +(defmethod nextage-interface + (:init (&rest args) + (send-super :init :robot nextageopen-robot)) + (:default-controller () + (append + (send self :larm-controller) + (send self :rarm-controller) + (send self :torso-controller) + (send self :head-controller))) + (:larm-controller () + (list + (list + (cons :controller-action "/larm_controller/follow_joint_trajectory_action") + (cons :controller-state "/larm_controller/fstate") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (send-all (send robot :larm :joint-list) :name))))) + (:rarm-controller () + (list + (list + (cons :controller-action "/rarm_controller/follow_joint_trajectory_action") + (cons :controller-state "/rarm_controller/fstate") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (send-all (send robot :rarm :joint-list) :name))))) + (:torso-controller () + (list + (list + (cons :controller-action "/torso_controller/follow_joint_trajectory_action") + (cons :controller-state "/torso_controller/fstate") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (send-all (send robot :torso :joint-list) :name))))) + (:head-controller () + (list + (list + (cons :controller-action "/head_controller/follow_joint_trajectory_action") + (cons :controller-state "/head_controller/fstate") + (cons :action-type control_msgs::FollowJointTrajectoryAction) + (cons :joint-names (send-all (send robot :head :joint-list) :name))))) + ) + +(defun nextage-init () + (if (not (boundp '*ri*)) + (setq *ri* (instance nextage-interface :init))) + (if (not (boundp '*nextage*)) + (setq *nextage* (instance nextageopen-robot :init)))) diff --git a/nextage_tutorials/launch/sample.launch b/nextage_tutorials/launch/sample.launch new file mode 100644 index 00000000..458bef34 --- /dev/null +++ b/nextage_tutorials/launch/sample.launch @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + + + + diff --git a/nextage_tutorials/model/NextageKinect.xacro b/nextage_tutorials/model/NextageKinect.xacro new file mode 100644 index 00000000..14cf8780 --- /dev/null +++ b/nextage_tutorials/model/NextageKinect.xacro @@ -0,0 +1,82 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + 15.0 + + 1.047197 + + + B8G8R8 + 640 + 480 + + + 0.05 + 50 + + + + 0.1 + true + 15.0 + head_camera + /head_camera/rgb/image_raw + /head_camera/rgb/camera_info + /head_camera/depth_registered/image_raw + /head_camera/depth_registered/camera_info + /head_camera/depth_registered/points + head_camera_rgb_optical_frame + 0.35 + 4.5 + 0 + 0 + 0 + 0 + 0 + + + + diff --git a/nextage_tutorials/model/sample.world b/nextage_tutorials/model/sample.world new file mode 100644 index 00000000..46376eba --- /dev/null +++ b/nextage_tutorials/model/sample.world @@ -0,0 +1,19 @@ + + + + + + model://sun + + + + model://ground_plane + + + + model://cafe_table + + 1.0 0 0 0 0 0 + + + diff --git a/nextage_tutorials/package.xml b/nextage_tutorials/package.xml new file mode 100644 index 00000000..6fc36136 --- /dev/null +++ b/nextage_tutorials/package.xml @@ -0,0 +1,19 @@ + + + nextage_tutorials + 0.0.0 + nextage tutorials for EusLisp/Gazebo users + + BSD + Kei Okada + + catkin + + pr2eus + euscollada + nextage_description + nextage_gazebo + + + + diff --git a/nextage_tutorials/sample/sample.l b/nextage_tutorials/sample/sample.l new file mode 100755 index 00000000..72873d58 --- /dev/null +++ b/nextage_tutorials/sample/sample.l @@ -0,0 +1,28 @@ +#!/usr/bin/env roseus + +;; demo on euslisp + +(load "package://nextage_tutorials/nextage.l") + +(setq *nextage* (nextageopen)) +(send *nextage* :reset-pose) +(objects (list *nextage*)) + +(send *nextage* :head :neck-p :joint-angle 25) +(send *nextage* :head :neck-y :joint-angle 45) +(send *nextage* :torso :waist-y :joint-angle 30) + +(send *nextage* :larm :move-end-pos #f(0 0 200) :world :debug-view :no-message) +(send *nextage* :rarm :move-end-pos #f(0 -200 0) :world :debug-view :no-message) + +;; demo on gazebo +(load "package://nextage_tutorials/euslisp/nextage-interface.l") + +(nextage-init) +(send *ri* :angle-vector (send *nextage* :angle-vector) 1000) ;; caution !!! it could be too fast!!!! +(send *ri* :wait-interpolation) + +(send *nextage* :reset-pose) +(send *nextage* :head :neck-p :joint-angle 25) +(send *ri* :angle-vector (send *nextage* :angle-vector) 500) ;; caution !!! it could be too fast!!!! +(send *ri* :wait-interpolation)