Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[12028] pkg/trunk
|
From: <ge...@us...> - 2009-03-03 02:32:42
|
Revision: 12028
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=12028&view=rev
Author: gerkey
Date: 2009-03-03 02:32:36 +0000 (Tue, 03 Mar 2009)
Log Message:
-----------
Merged from tabletop_manipulation_feb-2009 branch. Mostly changes to
tabletop_manipulation package, with a smattering of fixes and tweaks to
other packges.
Modified Paths:
--------------
pkg/trunk/demos/2dnav_pr2/2dnav_pr2_nolocalization.xml
pkg/trunk/demos/tabletop_manipulation/launch/arm.launch
pkg/trunk/demos/tabletop_manipulation/launch/ekf.launch
pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch
pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py
pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py
pkg/trunk/demos/tabletop_manipulation/scripts/tuckarm.py
pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp
pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/teleop_joystick.launch
pkg/trunk/visualization/rviz/src/rviz/displays/collision_map_display.cpp
pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp
Added Paths:
-----------
pkg/trunk/demos/2dnav_pr2/maps/
pkg/trunk/demos/2dnav_pr2/maps/empty_0.05.pgm
pkg/trunk/demos/tabletop_manipulation/launch/hw.launch
pkg/trunk/demos/tabletop_manipulation/launch/nav.launch
pkg/trunk/demos/tabletop_manipulation/launch/r_gripper_effort_controller.xml
pkg/trunk/demos/tabletop_manipulation/launch/teleop.launch
pkg/trunk/demos/tabletop_manipulation/launch/torso_effort_controller.xml
pkg/trunk/demos/tabletop_manipulation/launch/trajectory_controllers.xml
pkg/trunk/demos/tabletop_manipulation/scripts/actuategripper.py
pkg/trunk/demos/tabletop_manipulation/scripts/actuatetorso.py
pkg/trunk/demos/tabletop_manipulation/scripts/detecttable.py
pkg/trunk/demos/tabletop_manipulation/scripts/recordstaticmap.py
pkg/trunk/demos/tabletop_manipulation/scripts/stopbase.py
Removed Paths:
-------------
pkg/trunk/demos/2dnav_pr2/maps/empty_0.05.pgm
pkg/trunk/demos/tabletop_manipulation/trajectory_controllers.xml
Modified: pkg/trunk/demos/2dnav_pr2/2dnav_pr2_nolocalization.xml
===================================================================
--- pkg/trunk/demos/2dnav_pr2/2dnav_pr2_nolocalization.xml 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/demos/2dnav_pr2/2dnav_pr2_nolocalization.xml 2009-03-03 02:32:36 UTC (rev 12028)
@@ -31,7 +31,7 @@
<node pkg="robot_pose_ekf" type="robot_pose_ekf" args="robot_pose_ekf" machine="four"/>
- <node pkg="map_server" type="map_server" args="$(find 2dnav_stage)/maps/willow-empty-0.05.pgm 0.05" respawn="true" machine="four" />
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_pr2)/maps/empty_0.05.pgm 0.05" respawn="true" machine="four" />
<node pkg="highlevel_controllers" type="move_base_navfn" respawn="true" machine="four" output="screen" name="move_base_node">
Deleted: pkg/trunk/demos/2dnav_pr2/maps/empty_0.05.pgm
===================================================================
(Binary files differ)
Copied: pkg/trunk/demos/2dnav_pr2/maps/empty_0.05.pgm (from rev 11953, pkg/branches/tabletop_manipulation_feb-2009/demos/2dnav_pr2/maps/empty_0.05.pgm)
===================================================================
(Binary files differ)
Modified: pkg/trunk/demos/tabletop_manipulation/launch/arm.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/arm.launch 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/demos/tabletop_manipulation/launch/arm.launch 2009-03-03 02:32:36 UTC (rev 12028)
@@ -1,6 +1,6 @@
<launch>
<!-- Arm trajectory controller -->
- <param name="right_arm_trajectory_controller/velocity_scaling_factor" type="double" value="0.25"/>
+ <param name="right_arm_trajectory_controller/velocity_scaling_factor" type="double" value="0.1"/>
<param name="right_arm_trajectory_controller/trajectory_wait_timeout" type="double" value=".25"/>
<param name="right_arm_trajectory_controller/r_shoulder_pan_joint/goal_reached_threshold" type="double" value="0.1"/>
@@ -27,5 +27,11 @@
<node pkg="mechanism_control" type="spawner.py" args="$(find tabletop_manipulation)/launch/trajectory_controllers.xml" output="screen"/>
+ <!-- Gripper effort controller -->
+ <node pkg="mechanism_control" type="spawner.py" args="$(find tabletop_manipulation)/launch/r_gripper_effort_controller.xml" output="screen"/>
+
+ <!-- Torso effort controller -->
+ <node pkg="mechanism_control" type="spawner.py" args="$(find tabletop_manipulation)/launch/torso_effort_controller.xml" output="screen"/>
+
</launch>
Modified: pkg/trunk/demos/tabletop_manipulation/launch/ekf.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/ekf.launch 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/demos/tabletop_manipulation/launch/ekf.launch 2009-03-03 02:32:36 UTC (rev 12028)
@@ -1,4 +1,5 @@
<launch>
+ <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
<param name="robot_pose_ekf/freq" value="30.0"/>
<param name="robot_pose_ekf/sensor_timeout" value="1.0"/>
@@ -6,5 +7,6 @@
<param name="robot_pose_ekf/imu_used" value="true"/>
<param name="robot_pose_ekf/vo_used" value="false"/>
- <node pkg="robot_pose_ekf" type="robot_pose_ekf" args="robot_pose_ekf"/>
+ <node pkg="robot_pose_ekf" type="robot_pose_ekf" args="robot_pose_ekf"
+ machine="four"/>
</launch>
Added: pkg/trunk/demos/tabletop_manipulation/launch/hw.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/hw.launch (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/launch/hw.launch 2009-03-03 02:32:36 UTC (rev 12028)
@@ -0,0 +1,20 @@
+<launch>
+ <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
+
+ <!--include file="$(find tabletop_manipulation)/launch/teleop.launch"/-->
+
+ <!-- We're working without a map, relying on IMU-corrected wheel odom -->
+ <param name="/global_frame_id" value="odom_combined_offset"/>
+ <node pkg="tf" type="transform_sender" args="-20 -20 0 0 0 0 odom_combined_offset odom_combined 10"/>
+
+ <!-- Everything depends on ekf.launch, because it defines the
+ odom_combined frame -->
+ <include file="$(find tabletop_manipulation)/launch/ekf.launch"/>
+
+ <include file="$(find tabletop_manipulation)/launch/controllers.launch"/>
+ <include file="$(find tabletop_manipulation)/launch/perception.launch"/>
+ <!-- Note that nav.launch requires perception.launch, because the latter
+ runs the filters on the tilt laser scans -->
+ <include file="$(find tabletop_manipulation)/launch/nav.launch"/>
+ <!--include file="$(find tabletop_manipulation)/launch/planning.launch"/-->
+</launch>
Added: pkg/trunk/demos/tabletop_manipulation/launch/nav.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/nav.launch (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/launch/nav.launch 2009-03-03 02:32:36 UTC (rev 12028)
@@ -0,0 +1,71 @@
+<launch>
+ <group name="wg">
+ <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
+
+ <node pkg="map_server" type="map_server" args="$(find 2dnav_pr2)/maps/empty_0.05.pgm 0.05" respawn="true"/>
+
+
+ <node name="move_base" pkg="highlevel_controllers" type="move_base_navfn" respawn="true" output="screen" machine="three">
+ <remap from="tilt_laser_cloud_filtered" to="robotlinks_cloud_filtered"/>
+
+ <param name="move_base/environmentList" value="2D,3DKIN"/> <!-- future extension -->
+ <param name="move_base/plannerList" value="ADPlanner,ARAPlanner"/> <!-- future extension -->
+ <param name="move_base/plannerType" value="ARAPlanner"/>
+ <param name="move_base/environmentType" value="2D"/>
+ <param name="move_base/controller_frequency" value="30.0"/>
+ <param name="move_base/map_update_frequency" value="2.0"/>
+ <param name="move_base/planner_frequency" value="0.0"/>
+ <param name="move_base/plannerTimeLimit" value="0.5"/>
+ <param name="costmap_2d/base_laser_max_range" value="20.0"/>
+ <param name="costmap_2d/tilt_laser_max_range" value="3.0"/>
+ <param name="costmap_2d/lethal_obstacle_threshold" value="100.0"/>
+ <param name="costmap_2d/z_threshold_max" value="2.0"/>
+ <param name="costmap_2d/z_threshold_min" value="0.13"/>
+ <param name="costmap_2d/no_information_value" value="255"/>
+ <param name="costmap_2d/freespace_projection_height" value="2.0"/>
+ <param name="costmap_2d/inflation_radius" value="0.65"/>
+ <param name="costmap_2d/circumscribed_radius" value="0.46"/>
+ <param name="costmap_2d/inscribed_radius" value="0.325"/>
+ <param name="costmap_2d/raytrace_window" value="10.0"/>
+ <param name="costmap_2d/weight" value="0.1"/>
+
+
+ <param name="trajectory_rollout/map_size" value="4.0" />
+ <param name="trajectory_rollout/acc_limit_x" value="2.5" />
+ <param name="trajectory_rollout/acc_limit_y" value="2.5" />
+ <param name="trajectory_rollout/acc_limit_th" value="3.2" />
+ <param name="trajectory_rollout/sim_time" value="1.0" />
+ <param name="trajectory_rollout/sim_granularity" value="0.05" />
+ <param name="trajectory_rollout/vx_samples" value="15" />
+ <param name="trajectory_rollout/vtheta_samples" value="15" />
+ <param name="trajectory_rollout/path_distance_bias" value="0.6" />
+ <param name="trajectory_rollout/goal_distance_bias" value="0.8" />
+ <param name="trajectory_rollout/occdist_scale" value="0.2" />
+ <param name="trajectory_rollout/heading_lookahead" value="0.325" />
+ <param name="trajectory_rollout/oscillation_reset_dist" value="0.05" />
+ <param name="trajectory_rollout/holonomic_robot" value="true" />
+ <param name="trajectory_rollout/max_vel_x" value="0.5" />
+ <param name="trajectory_rollout/min_vel_x" value="0.1" />
+ <param name="trajectory_rollout/max_vel_th" value="1.0" />
+ <param name="trajectory_rollout/min_vel_th" value="-1.0" />
+ <param name="trajectory_rollout/min_in_place_vel_th" value="0.4" />
+ <param name="trajectory_rollout/freespace_model" value="true" />
+ <param name="trajectory_rollout/dwa" value="false" />
+ <param name="trajectory_rollout/simple_attractor" value="false" />
+
+ <param name="trajectory_rollout/yaw_goal_tolerance" value=".05" />
+ <param name="trajectory_rollout/xy_goal_tolerance" value=".1" />
+
+ <!-- Forces a check that we are receiving base scans that are correctly up to date at a rate of at least 2 Hz. This is
+ way too slow for the real system, but necessary for rosstage, which does not reliably meet a 5Hz update rate. -->
+ <param name="costmap_2d/base_laser_update_rate" value="2.0"/>
+
+ <!-- Setting these parameters to 0.0 disables the watchdog on them. For stage this is required since we
+ are not getting any data -->
+ <param name="costmap_2d/tilt_laser_update_rate" value="2.0"/>
+ <param name="costmap_2d/low_obstacle_update_rate" value="0.0"/>
+ <param name="costmap_2d/stereo_update_rate" value="0.0"/>
+ </node>
+
+ </group>
+</launch>
Modified: pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/demos/tabletop_manipulation/launch/nav_sim.launch 2009-03-03 02:32:36 UTC (rev 12028)
@@ -15,6 +15,9 @@
<param name="move_base/map_update_frequency" value="2.0"/>
<param name="move_base/planner_frequency" value="0.0"/>
<param name="move_base/plannerTimeLimit" value="0.5"/>
+ <param name="move_base/trans_stopped_velocity" value="0.01"/>
+ <param name="move_base/rot_stopped_velocity" value="0.01"/>
+
<param name="costmap_2d/base_laser_max_range" value="20.0"/>
<param name="costmap_2d/tilt_laser_max_range" value="3.0"/>
<param name="costmap_2d/lethal_obstacle_threshold" value="100.0"/>
Added: pkg/trunk/demos/tabletop_manipulation/launch/r_gripper_effort_controller.xml
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/r_gripper_effort_controller.xml (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/launch/r_gripper_effort_controller.xml 2009-03-03 02:32:36 UTC (rev 12028)
@@ -0,0 +1,8 @@
+<?xml version="1.0"?>
+
+<controllers>
+ <controller name="right_gripper/effort_controller" type="JointEffortControllerNode">
+ <joint name="r_gripper_l_finger_joint" />
+ </controller>
+</controllers>
+
Added: pkg/trunk/demos/tabletop_manipulation/launch/teleop.launch
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/teleop.launch (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/launch/teleop.launch 2009-03-03 02:32:36 UTC (rev 12028)
@@ -0,0 +1,17 @@
+<launch>
+ <group name="wg">
+ <include file="$(find pr2_alpha)/$(env ROBOT).machine" />
+
+ <param name="axis_vx" value="3" type="int"/>
+ <param name="axis_vy" value="2" type="int"/>
+ <param name="axis_vw" value="0" type="int"/>
+ <param name="pan" value="4" type="int"/>
+ <param name="tilt" value="5" type="int"/>
+ <param name="max_vw" value="1.0" />
+ <param name="max_vx" value="1.0" />
+ <param name="max_vy" value="1.0" />
+ <param name="deadman_button" value="4" type="int"/>
+ <node pkg="teleop_base" type="teleop_base" args="--deadman_no_publish" respawn="true" machine="four" />
+
+ </group>
+</launch>
Added: pkg/trunk/demos/tabletop_manipulation/launch/torso_effort_controller.xml
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/torso_effort_controller.xml (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/launch/torso_effort_controller.xml 2009-03-03 02:32:36 UTC (rev 12028)
@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+
+<controllers>
+ <controller name="torso_lift/effort_controller" type="JointEffortControllerNode">
+ <joint name="torso_lift_joint" />
+ </controller>
+</controllers>
+
+
Added: pkg/trunk/demos/tabletop_manipulation/launch/trajectory_controllers.xml
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/launch/trajectory_controllers.xml (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/launch/trajectory_controllers.xml 2009-03-03 02:32:36 UTC (rev 12028)
@@ -0,0 +1,112 @@
+<?xml version="1.0"?>
+
+<controllers>
+ <!-- ========================================= -->
+ <!-- right arm array -->
+ <controller name="right_arm_trajectory_controller" type="ArmTrajectoryControllerNode">
+ <listen_topic name="right_arm_trajectory_command" />
+ <kinematics>
+ <elem key="kdl_chain_name">right_arm</elem>
+ </kinematics>
+ <map name="controller_param">
+ <elem key="kdl_chain_name">right_arm</elem>
+ </map>
+ <controller name="r_shoulder_pan_controller" topic="r_shoulder_pan_controller" type="JointPDController">
+ <joint name="r_shoulder_pan_joint" >
+ <pid p="140.0" d="30.0" i="0.8" iClamp="100.0" />
+ </joint>
+ </controller>
+ <controller name="r_shoulder_lift_controller" topic="r_shoulder_pitch_controller" type="JointPDController">
+ <joint name="r_shoulder_lift_joint" >
+ <pid p="70.0" d="20.0" i="0.0" iClamp="100.0" />
+ </joint>
+ </controller>
+ <controller name="r_upperarm_roll_controller" topic="r_upperarm_roll_controller" type="JointPDController">
+ <joint name="r_upper_arm_roll_joint" >
+ <pid p="50.0" d="5.0" i="0.2" iClamp="100.0" />
+ </joint>
+ </controller>
+ <controller name="r_elbow_controller" topic="r_elbow_flex_controller" type="JointPDController">
+ <joint name="r_elbow_flex_joint" >
+ <pid p="100.0" d="5.0" i="0.5" iClamp="100.0" />
+ </joint>
+ </controller>
+ <controller name="r_forearm_roll_controller" topic="r_forearm_roll_controller" type="JointPDController"> <joint name="r_forearm_roll_joint"
+>
+ <pid p="10.0" d="2.0" i="0.1" iClamp="10.0" />
+ </joint>
+ </controller>
+
+ <controller name="r_wrist_flex_controller" type="JointPDController">
+ <joint name="r_wrist_flex_joint">
+ <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
+ </joint> </controller>
+
+ <controller name="r_wrist_roll_controller" type="JointPDController">
+ <joint name="r_wrist_roll_joint">
+ <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
+ </joint> </controller>
+
+ <!--controller name="r_gripper_l_finger_controller" type="JointPDController">
+ <joint name="r_gripper_l_finger_joint">
+ <pid p="100" i="0.1" d="0.5" iClamp="100.0" />
+ </joint> </controller-->
+
+ <trajectory interpolation="cubic" />
+
+ </controller>
+
+ <controller name="left_arm_trajectory_controller" type="ArmTrajectoryControllerNode">
+ <listen_topic name="left_arm_trajectory_command" />
+ <kinematics>
+ <elem key="kdl_chain_name">left_arm</elem>
+ </kinematics>
+ <map name="controller_param">
+ <elem key="kdl_chain_name">left_arm</elem>
+ </map>
+ <controller name="l_shoulder_pan_controller" topic="l_shoulder_pan_controller" type="JointPDController">
+ <joint name="l_shoulder_pan_joint" >
+ <pid p="140.0" d="30.0" i="0.8" iClamp="100.0" />
+ </joint>
+ </controller>
+ <controller name="l_shoulder_lift_controller" topic="l_shoulder_pitch_controller" type="JointPDController">
+ <joint name="l_shoulder_lift_joint" >
+ <pid p="70.0" d="20.0" i="0.0" iClamp="100.0" />
+ </joint>
+ </controller>
+ <controller name="l_upperarm_roll_controller" topic="l_upperarm_roll_controller" type="JointPDController">
+ <joint name="l_upper_arm_roll_joint" >
+ <pid p="50.0" d="5.0" i="0.2" iClamp="100.0" />
+ </joint>
+ </controller>
+ <controller name="l_elbow_controller" topic="l_elbow_flex_controller" type="JointPDController">
+ <joint name="l_elbow_flex_joint" >
+ <pid p="100.0" d="5.0" i="0.5" iClamp="100.0" />
+ </joint>
+ </controller>
+ <controller name="l_forearm_roll_controller" topic="l_forearm_roll_controller" type="JointPDController"> <joint name="l_forearm_roll_joint"
+>
+ <pid p="10.0" d="2.0" i="0.1" iClamp="10.0" />
+ </joint>
+ </controller>
+
+ <controller name="l_wrist_flex_controller" type="JointPDController">
+ <joint name="l_wrist_flex_joint">
+ <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
+ </joint> </controller>
+
+ <controller name="l_wrist_roll_controller" type="JointPDController">
+ <joint name="l_wrist_roll_joint">
+ <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
+ </joint> </controller>
+
+ <!--controller name="l_gripper_l_finger_controller" type="JointPDController">
+ <joint name="l_gripper_l_finger_joint">
+ <pid p="100" i="0.1" d="0.5" iClamp="100.0" />
+ </joint> </controller-->
+
+ <trajectory interpolation="cubic" />
+
+ </controller>
+</controllers>
+
Added: pkg/trunk/demos/tabletop_manipulation/scripts/actuategripper.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/actuategripper.py (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/actuategripper.py 2009-03-03 02:32:36 UTC (rev 12028)
@@ -0,0 +1,93 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Revision $Id: rossync 3844 2009-02-16 19:49:10Z gerkey $
+
+import roslib
+roslib.load_manifest('tabletop_manipulation')
+import rospy
+from std_msgs.msg import Float64
+
+import sys
+
+class ActuateGripper:
+ def __init__(self, side):
+ self.side = side
+ self.status = None
+ self.gripper_close_effort = -0.5
+ self.gripper_open_effort = 6.0
+ self.pub = rospy.Publisher(self.side + '_gripper/effort_controller/set_command', Float64)
+ #rospy.Subscriber(self.side + '_arm_state', MoveArmState, self.movearmCallback)
+
+ def actuateGripper(self, open):
+ msg = Float64()
+ if open:
+ print '[ActuateGripper] Opening gripper...'
+ msg.data = self.gripper_open_effort
+ else:
+ print '[ActuateGripper] Closing gripper...'
+ msg.data = self.gripper_close_effort
+ self.pub.publish(msg)
+
+ # HACK: don't know what to monitor to determine that the gripper is
+ # done opening or closing
+ print '[ActuateGripper] Waiting for gripper to finish moving...'
+ rospy.sleep(4.0)
+
+ return True
+
+USAGE = 'actuategripper.py {right|left} {open|close}'
+if __name__ == '__main__':
+ if len(sys.argv) != 3 or \
+ (sys.argv[1] != 'right' and sys.argv[1] != 'left') or \
+ (sys.argv[2] != 'open' and sys.argv[2] != 'close'):
+ print USAGE
+ sys.exit(-1)
+
+ side = sys.argv[1]
+ open = sys.argv[2] == 'open'
+
+ ag = ActuateGripper(side)
+
+ rospy.init_node('actuate_gripper', anonymous=True)
+
+ # HACK
+ import time
+ time.sleep(2.0)
+
+ res = ag.actuateGripper(open)
+
+ if res:
+ print 'Success!'
+ else:
+ print 'Failure!'
Added: pkg/trunk/demos/tabletop_manipulation/scripts/actuatetorso.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/actuatetorso.py (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/actuatetorso.py 2009-03-03 02:32:36 UTC (rev 12028)
@@ -0,0 +1,90 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Revision $Id: rossync 3844 2009-02-16 19:49:10Z gerkey $
+
+import roslib
+roslib.load_manifest('tabletop_manipulation')
+import rospy
+from std_msgs.msg import Float64
+
+import sys
+
+class ActuateTorso:
+ def __init__(self):
+ self.status = None
+ self.torso_down_effort = -4000.0
+ self.torso_up_effort = 4000.0
+ self.pub = rospy.Publisher('torso_lift/effort_controller/set_command', Float64)
+ #rospy.Subscriber(self.side + '_arm_state', MoveArmState, self.movearmCallback)
+
+ def actuateTorso(self, up):
+ msg = Float64()
+ if up:
+ print '[ActuateTorso] Raising torso...'
+ msg.data = self.torso_up_effort
+ else:
+ print '[ActuateTorso] Lowering torso...'
+ msg.data = self.torso_down_effort
+ self.pub.publish(msg)
+
+ # HACK: don't know what to monitor to determine that the torso is
+ # done raising or lowering
+ print '[ActuateTorso] Waiting for torso to finish moving...'
+ rospy.sleep(4.0)
+
+ return True
+
+USAGE = 'actuatetorso.py {up|down}'
+if __name__ == '__main__':
+ if len(sys.argv) != 2 or \
+ (sys.argv[1] != 'up' and sys.argv[1] != 'down'):
+ print USAGE
+ sys.exit(-1)
+
+ up = sys.argv[1] == 'up'
+
+ ag = ActuateTorso()
+
+ rospy.init_node('actuate_torso', anonymous=True)
+
+ # HACK
+ import time
+ time.sleep(2.0)
+
+ res = ag.actuateTorso(up)
+
+ if res:
+ print 'Success!'
+ else:
+ print 'Failure!'
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/approachtable.py 2009-03-03 02:32:36 UTC (rev 12028)
@@ -115,6 +115,7 @@
if not ts.tiltScan(2.0):
print '[ApproachTable] Failed to change tilt scan'
return False
+
# Call out to blocking MoveBase
return self.mb.moveBase(resp.table.header.frame_id,
approach_pose[0],
@@ -302,8 +303,8 @@
if at.approachTable(1.0, True):
res = at.approachTable(0.4, True)
- if res:
+ if not res:
+ print 'Failure!'
+ else:
print 'Success!'
- else:
- print 'Failure!'
Added: pkg/trunk/demos/tabletop_manipulation/scripts/detecttable.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/detecttable.py (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/detecttable.py 2009-03-03 02:32:36 UTC (rev 12028)
@@ -0,0 +1,86 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Revision $Id: rossync 3844 2009-02-16 19:49:10Z gerkey $
+
+import roslib
+roslib.load_manifest('tabletop_manipulation')
+import rospy
+from robot_srvs.srv import FindTable, FindTableRequest
+
+import sys
+from math import *
+
+class DetectTable:
+ def __init__(self):
+ pass
+
+ def detectTable(self):
+ print '[DetectTable] Waiting for table_object_detector service...'
+ rospy.wait_for_service('table_object_detector')
+ print '[DetectTable] Calling table_object_detector service...'
+ s = rospy.ServiceProxy('table_object_detector', FindTable)
+ resp = s.call(FindTableRequest())
+ print '[DetectTable] Result:'
+ print '[DetectTable] Table (frame %s): %f %f %f %f' % (resp.table.header.frame_id,
+ resp.table.table_min.x,
+ resp.table.table_max.x,
+ resp.table.table_min.y,
+ resp.table.table_max.y)
+ print '[DetectTable] %d objects detected on table' % len(resp.table.objects)
+ for o in resp.table.objects:
+ print '[DetectTable] (%f %f %f): %f %f %f' % \
+ (o.center.x, o.center.y, o.center.z,
+ o.max_bound.x - o.min_bound.x,
+ o.max_bound.y - o.min_bound.y,
+ o.max_bound.z - o.min_bound.z)
+
+ print '[DetectTable] Poly:'
+ for p in resp.table.table.points:
+ print '[DetectTable] %f %f %f'%(p.x,p.y,p.z)
+
+ return resp
+
+if __name__ == '__main__':
+ dt = DetectTable()
+
+ rospy.init_node('detect_table', anonymous=True)
+
+ res = dt.detectTable()
+
+ if not res:
+ print 'Failure!'
+ else:
+ print 'Success!'
+
+
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/movearm.py 2009-03-03 02:32:36 UTC (rev 12028)
@@ -144,7 +144,7 @@
ma = MoveArm(side)
- rospy.init_node('talker', anonymous=True)
+ rospy.init_node('move_arm', anonymous=True)
# HACK
import time
Added: pkg/trunk/demos/tabletop_manipulation/scripts/recordstaticmap.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/recordstaticmap.py (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/recordstaticmap.py 2009-03-03 02:32:36 UTC (rev 12028)
@@ -0,0 +1,77 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Revision $Id: rossync 3844 2009-02-16 19:49:10Z gerkey $
+
+import roslib
+roslib.load_manifest('tabletop_manipulation')
+import rospy
+from robot_srvs.srv import RecordStaticMapTrigger, RecordStaticMapTriggerRequest
+
+from tiltscan import TiltScan
+
+import sys
+from math import *
+
+class RecordStaticMap:
+ def __init__(self, ts):
+ self.ts = ts
+
+ def recordStaticMap(self):
+ resp = self.ts.tiltScan(10.0)
+ if not resp:
+ print '[ApproachTable] Failed to change scan rate'
+ return False
+
+ print '[RecordStaticMap] Waiting for service: collision_map_buffer/record_static_map'
+ rospy.wait_for_service('collision_map_buffer/record_static_map')
+ print '[RecordStaticMap] Calling collision_map_buffer/record_static_map service...'
+ s = rospy.ServiceProxy('collision_map_buffer/record_static_map', RecordStaticMapTrigger)
+ resp = s.call(RecordStaticMapTriggerRequest(roslib.rostime.Time().from_seconds(resp.time)))
+ print '[RecordStaticMap] response: %d' % resp.status
+ return resp
+
+if __name__ == '__main__':
+ ts = TiltScan('laser_tilt_controller', 5.0)
+ rsm = RecordStaticMap(ts)
+
+ rospy.init_node('record_static_map', anonymous=True)
+
+ res = rsm.recordStaticMap()
+
+ if not res:
+ print 'Failure!'
+ else:
+ print 'Success!'
+
+
Added: pkg/trunk/demos/tabletop_manipulation/scripts/stopbase.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/stopbase.py (rev 0)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/stopbase.py 2009-03-03 02:32:36 UTC (rev 12028)
@@ -0,0 +1,85 @@
+#!/usr/bin/env python
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2009, Willow Garage, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+# notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+# copyright notice, this list of conditions and the following
+# disclaimer in the documentation and/or other materials provided
+# with the distribution.
+# * Neither the name of the Willow Garage nor the names of its
+# contributors may be used to endorse or promote products derived
+# from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Revision $Id: rossync 3844 2009-02-16 19:49:10Z gerkey $
+
+import roslib
+roslib.load_manifest('tabletop_manipulation')
+import rospy
+from robot_msgs.msg import Planner2DGoal, Planner2DState
+
+class StopBase:
+ def __init__(self):
+ self.pub_goal = rospy.Publisher("goal", Planner2DGoal)
+ rospy.Subscriber("state", Planner2DState, self.stateCallback)
+ self.goal_id = -1
+ self.status = None
+
+ def stateCallback(self, msg):
+ self.goal_id = msg.goal_id
+ self.status = msg.status
+
+ def stopBase(self):
+ g = Planner2DGoal()
+ g.goal.x = 0
+ g.goal.y = 0
+ g.goal.th = 0
+ g.enable = 1
+ g.header.frame_id = ''
+ g.timeout = 0.0
+ print '[StopBase] Stopping base'
+ self.pub_goal.publish(g)
+
+ while self.status == None or self.status == Planner2DState.ACTIVE:
+ print '[StopBase] Waiting for goal achievement...'
+ rospy.sleep(1.0)
+
+ return self.status != Planner2DState.INACTIVE
+
+if __name__ == '__main__':
+ import sys
+ if len(sys.argv) != 1:
+ print 'wrong # args'
+ sys.exit(-1)
+
+ sb = StopBase()
+
+ rospy.init_node('stop_base', anonymous=True)
+
+ res = sb.stopBase()
+
+ if res:
+ print 'Success!'
+ else:
+ print 'Failure!'
+
Modified: pkg/trunk/demos/tabletop_manipulation/scripts/tuckarm.py
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/scripts/tuckarm.py 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/demos/tabletop_manipulation/scripts/tuckarm.py 2009-03-03 02:32:36 UTC (rev 12028)
@@ -66,10 +66,10 @@
if self.side == 'left':
self.positions[1][self.joint_names[self.side[0] + '_shoulder_lift_joint']] = 1.57
- self.positions[1][self.joint_names[self.side[0] + '_upper_arm_roll_joint']] = 1.57
+ self.positions[1][self.joint_names[self.side[0] + '_upper_arm_roll_joint']] = 1.3
else:
self.positions[1][self.joint_names[self.side[0] + '_shoulder_lift_joint']] = 1.57
- self.positions[1][self.joint_names[self.side[0] + '_upper_arm_roll_joint']] = -1.57
+ self.positions[1][self.joint_names[self.side[0] + '_upper_arm_roll_joint']] = -1.3
print self.positions
Deleted: pkg/trunk/demos/tabletop_manipulation/trajectory_controllers.xml
===================================================================
--- pkg/trunk/demos/tabletop_manipulation/trajectory_controllers.xml 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/demos/tabletop_manipulation/trajectory_controllers.xml 2009-03-03 02:32:36 UTC (rev 12028)
@@ -1,112 +0,0 @@
-<?xml version="1.0"?>
-
-<controllers>
- <!-- ========================================= -->
- <!-- right arm array -->
- <controller name="right_arm_trajectory_controller" type="ArmTrajectoryControllerNode">
- <listen_topic name="right_arm_trajectory_command" />
- <kinematics>
- <elem key="kdl_chain_name">right_arm</elem>
- </kinematics>
- <map name="controller_param">
- <elem key="kdl_chain_name">right_arm</elem>
- </map>
- <controller name="r_shoulder_pan_controller" topic="r_shoulder_pan_controller" type="JointPDController">
- <joint name="r_shoulder_pan_joint" >
- <pid p="140.0" d="30.0" i="0.8" iClamp="100.0" />
- </joint>
- </controller>
- <controller name="r_shoulder_lift_controller" topic="r_shoulder_pitch_controller" type="JointPDController">
- <joint name="r_shoulder_lift_joint" >
- <pid p="70.0" d="20.0" i="0.0" iClamp="100.0" />
- </joint>
- </controller>
- <controller name="r_upperarm_roll_controller" topic="r_upperarm_roll_controller" type="JointPDController">
- <joint name="r_upper_arm_roll_joint" >
- <pid p="50.0" d="5.0" i="0.2" iClamp="100.0" />
- </joint>
- </controller>
- <controller name="r_elbow_controller" topic="r_elbow_flex_controller" type="JointPDController">
- <joint name="r_elbow_flex_joint" >
- <pid p="100.0" d="5.0" i="0.5" iClamp="100.0" />
- </joint>
- </controller>
- <controller name="r_forearm_roll_controller" topic="r_forearm_roll_controller" type="JointPDController"> <joint name="r_forearm_roll_joint"
->
- <pid p="10.0" d="2.0" i="0.1" iClamp="10.0" />
- </joint>
- </controller>
-
- <controller name="r_wrist_flex_controller" type="JointPDController">
- <joint name="r_wrist_flex_joint">
- <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
- </joint> </controller>
-
- <controller name="r_wrist_roll_controller" type="JointPDController">
- <joint name="r_wrist_roll_joint">
- <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
- </joint> </controller>
-
- <!--controller name="r_gripper_l_finger_controller" type="JointPDController">
- <joint name="r_gripper_l_finger_joint">
- <pid p="100" i="0.1" d="0.5" iClamp="100.0" />
- </joint> </controller-->
-
- <trajectory interpolation="cubic" />
-
- </controller>
-
- <controller name="left_arm_trajectory_controller" type="ArmTrajectoryControllerNode">
- <listen_topic name="left_arm_trajectory_command" />
- <kinematics>
- <elem key="kdl_chain_name">left_arm</elem>
- </kinematics>
- <map name="controller_param">
- <elem key="kdl_chain_name">left_arm</elem>
- </map>
- <controller name="l_shoulder_pan_controller" topic="l_shoulder_pan_controller" type="JointPDController">
- <joint name="l_shoulder_pan_joint" >
- <pid p="140.0" d="30.0" i="0.8" iClamp="100.0" />
- </joint>
- </controller>
- <controller name="l_shoulder_lift_controller" topic="l_shoulder_pitch_controller" type="JointPDController">
- <joint name="l_shoulder_lift_joint" >
- <pid p="70.0" d="20.0" i="0.0" iClamp="100.0" />
- </joint>
- </controller>
- <controller name="l_upperarm_roll_controller" topic="l_upperarm_roll_controller" type="JointPDController">
- <joint name="l_upper_arm_roll_joint" >
- <pid p="50.0" d="5.0" i="0.2" iClamp="100.0" />
- </joint>
- </controller>
- <controller name="l_elbow_controller" topic="l_elbow_flex_controller" type="JointPDController">
- <joint name="l_elbow_flex_joint" >
- <pid p="100.0" d="5.0" i="0.5" iClamp="100.0" />
- </joint>
- </controller>
- <controller name="l_forearm_roll_controller" topic="l_forearm_roll_controller" type="JointPDController"> <joint name="l_forearm_roll_joint"
->
- <pid p="10.0" d="2.0" i="0.1" iClamp="10.0" />
- </joint>
- </controller>
-
- <controller name="l_wrist_flex_controller" type="JointPDController">
- <joint name="l_wrist_flex_joint">
- <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
- </joint> </controller>
-
- <controller name="l_wrist_roll_controller" type="JointPDController">
- <joint name="l_wrist_roll_joint">
- <pid p="10" i="0.1" d="0.5" iClamp="10.0" />
- </joint> </controller>
-
- <!--controller name="l_gripper_l_finger_controller" type="JointPDController">
- <joint name="l_gripper_l_finger_joint">
- <pid p="100" i="0.1" d="0.5" iClamp="100.0" />
- </joint> </controller-->
-
- <trajectory interpolation="cubic" />
-
- </controller>
-</controllers>
-
Modified: pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp
===================================================================
--- pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/highlevel/highlevel_controllers/src/move_arm.cpp 2009-03-03 02:32:36 UTC (rev 12028)
@@ -93,6 +93,8 @@
#include <urdf/URDF.h>
+#include <angles/angles.h>
+
#include <std_msgs/Empty.h>
#include <robot_msgs/JointTraj.h>
#include <pr2_mechanism_controllers/TrajectoryStart.h>
@@ -135,7 +137,11 @@
bool new_trajectory_;
bool plan_valid_;
robot_msgs::KinematicPath current_trajectory_;
+ double angle_tolerance_;
+ // HACK: this map should not be necessary
+ std::map<std::string, int> joints_;
+
// HighlevelController interface that we must implement
virtual void updateGoalMsg();
virtual bool makePlan();
@@ -149,10 +155,12 @@
void stopArm(void);
void getTrajectoryMsg(robot_msgs::KinematicPath &path,
robot_msgs::JointTraj &traj);
+ void displayKinematicPath(robot_msgs::KinematicPath &path);
void printKinematicPath(robot_msgs::KinematicPath &path);
void mechanismStateCallback();
void receiveStatus();
void getCurrentStateAsTraj(robot_msgs::JointTraj& traj);
+ bool isTrajectoryDone();
};
MoveArm::~MoveArm() {
@@ -209,12 +217,25 @@
this,
1);
ros::Node::instance()->advertise<robot_msgs::JointTraj>(controller_topic_, 1);
+ ros::Node::instance()->advertise<robot_msgs::DisplayKinematicPath>("display_kinematic_path", 10);
+
+ // HACK: I can't for the life of me figure out how to determine this ordering
+ // from the model.
+ angle_tolerance_ = 0.1;
+ joints_["r_shoulder_pan_joint"] = 0;
+ joints_["r_shoulder_lift_joint"] = 1;
+ joints_["r_upper_arm_roll_joint"] = 2;
+ joints_["r_elbow_flex_joint"] = 3;
+ joints_["r_forearm_roll_joint"] = 4;
+ joints_["r_wrist_flex_joint"] = 5;
+ joints_["r_wrist_roll_joint"] = 6;
}
void MoveArm::receiveStatus(void)
{
- ROS_DEBUG("Got status: %d %d %d",
- plan_id_, plan_status_.id, plan_status_.path.states.empty());
+ //ROS_DEBUG("Got status: %d %d %d %d",
+ //plan_id_, plan_status_.id, plan_status_.done,
+ //plan_status_.path.states.empty());
// HACK: sometimes we get the status for the new plan before we receive
// the response to the service call that gives us the new plan id. It
// happens that the planner increments the id by 1 with each request.
@@ -223,7 +244,8 @@
!plan_status_.path.states.empty())
{
current_trajectory_ = plan_status_.path;
- ROS_DEBUG("Got new trajectory from planner");
+ ROS_DEBUG("Got new trajectory from planner; distance: %.3f",
+ plan_status_.distance);
new_trajectory_ = true;
}
}
@@ -431,6 +453,61 @@
return getDone();
}
+bool MoveArm::isTrajectoryDone()
+{
+ // Assume that plan_status_.lock() was already acquired by the caller
+ bool done = false;
+ //if(!plan_status_.done)
+ //{
+ ROS_DEBUG("Checking for trajectory completion");
+ pr2_mechanism_controllers::TrajectoryQuery::Request req;
+ pr2_mechanism_controllers::TrajectoryQuery::Response res;
+ req.trajectoryid = traj_id_;
+ if(!ros::service::call(controller_name_ + "/TrajectoryQuery", req, res))
+ ROS_ERROR("Failed to query trajectory completion");
+ else if(res.done == res.State_Done)
+ done = true;
+
+ // The trajectory controller often has trouble declaring completion, so
+ // we'll also check directly.
+ if(!done)
+ {
+ done = true;
+ mechanism_state_msg_.lock();
+ for (std::map<std::string,int>::iterator it = joints_.begin();
+ it != joints_.end();
+ ++it)
+ {
+ unsigned int ind = 0;
+ while(mechanism_state_msg_.joint_states[ind].name != it->first && ind < mechanism_state_msg_.get_joint_states_size())
+ ind++;
+ if (ind == mechanism_state_msg_.get_joint_states_size())
+ {
+ ROS_ERROR("No joint '%s' in mechanism data.", it->first.c_str());
+ mechanism_state_msg_.unlock();
+ return false;
+ }
+
+ double diff = angles::shortest_angular_distance(mechanism_state_msg_.joint_states[ind].position,
+ current_trajectory_.states[current_trajectory_.states.size()-1].vals[it->second]);
+ ROS_DEBUG("Joint %s diff: %.3f", it->first.c_str(), diff);
+
+ if(fabs(diff) > angle_tolerance_)
+ {
+ done = false;
+ break;
+ }
+ }
+ mechanism_state_msg_.unlock();
+ }
+
+ //}
+ //else
+ //done = true;
+
+ return done;
+}
+
bool MoveArm::dispatchCommands()
{
// Force replanning if we have a new goal
@@ -451,23 +528,8 @@
}
else if(plan_status_.done || (traj_id_ >= 0 && !new_trajectory_))
{
- bool done = false;
- if(!plan_status_.done)
+ if(isTrajectoryDone())
{
- ROS_DEBUG("Checking for trajectory completion");
- pr2_mechanism_controllers::TrajectoryQuery::Request req;
- pr2_mechanism_controllers::TrajectoryQuery::Response res;
- req.trajectoryid = traj_id_;
- if(!ros::service::call(controller_name_ + "/TrajectoryQuery", req, res))
- ROS_ERROR("Failed to query trajectory completion");
- else if(res.done == res.State_Done)
- done = true;
- }
- else
- done = true;
-
- if(done)
- {
ROS_DEBUG("Plan completed.");
plan_id_ = -1;
traj_id_ = -1;
@@ -478,6 +540,7 @@
}
else if(plan_status_.valid &&
!plan_status_.unsafe &&
+ (plan_status_.distance < 0.1) &&
!current_trajectory_.states.empty())
{
ROS_DEBUG("sending new trajectory");
@@ -487,13 +550,7 @@
}
else
{
- ROS_DEBUG("Plan invalid %d %d %d %d %d %d\n",
- plan_id_ >= 0,
- plan_status_.id == plan_id_,
- plan_status_.valid,
- !plan_status_.unsafe,
- !current_trajectory_.states.empty(),
- new_trajectory_ == true);
+ ROS_DEBUG("Plan invalid or unsafe");
stopArm();
plan_valid_ = false;
}
@@ -517,6 +574,16 @@
ROS_DEBUG(ss.str().c_str());
}
+void MoveArm::displayKinematicPath(robot_msgs::KinematicPath &path)
+{
+ robot_msgs::DisplayKinematicPath dpath;
+ dpath.frame_id = "base_link";
+ dpath.model_name = kinematic_model_;
+ //currentState(dpath.start_state);
+ dpath.path = path;
+ ros::Node::instance()->publish("display_kinematic_path", dpath);
+}
+
void MoveArm::getTrajectoryMsg(robot_msgs::KinematicPath &path,
robot_msgs::JointTraj &traj)
{
@@ -538,6 +605,7 @@
void MoveArm::sendArmCommand(robot_msgs::KinematicPath &path,
const std::string &model)
{
+ displayKinematicPath(path);
stopArm();
ROS_DEBUG("Sending %d-step trajectory\n", path.states.size());
pr2_mechanism_controllers::TrajectoryStart::Request req;
Modified: pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp
===================================================================
--- pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/mapping/collision_map/src/collision_map_buffer.cpp 2009-03-03 02:32:36 UTC (rev 12028)
@@ -524,7 +524,7 @@
return;
}
- ROS_INFO ("End effector position is: [%f, %f, %f].", ee_global.point.x, ee_global.point.y, ee_global.point.z);
+ ROS_DEBUG ("End effector position is: [%f, %f, %f].", ee_global.point.x, ee_global.point.y, ee_global.point.z);
// Compute the leaves
vector<Leaf> object_leaves;
@@ -542,7 +542,7 @@
void
cloud_cb ()
{
- ROS_INFO ("Received %u data points.", (unsigned int)cloud_.pts.size ());
+ ROS_DEBUG ("Received %u data points.", (unsigned int)cloud_.pts.size ());
// Get the new parameters from the server
m_lock_.lock ();
@@ -587,7 +587,7 @@
t2 = ros::Time::now();
//time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
time_spent = (t2 - t1).toSec();
- ROS_INFO ("Static collision map computed in %g seconds. Number of boxes: %u.", time_spent, (unsigned int)static_leaves_.size ());
+ ROS_DEBUG ("Static collision map computed in %g seconds. Number of boxes: %u.", time_spent, (unsigned int)static_leaves_.size ());
}
else
@@ -639,7 +639,7 @@
t2 = ros::Time::now();
//time_spent = t2.tv_sec + (double)t2.tv_usec / 1000000.0 - (t1.tv_sec + (double)t1.tv_usec / 1000000.0);
time_spent = (t2 - t1).toSec();
- ROS_INFO ("Collision map with %u boxes computed in %g seconds. Total maps in the queue %d.",
+ ROS_DEBUG ("Collision map with %u boxes computed in %g seconds. Total maps in the queue %d.",
(unsigned int)final_collision_map_.boxes.size (), time_spent, decaying_maps_.size ());
publish ("collision_map_buffer", final_collision_map_);
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-03-03 02:32:36 UTC (rev 12028)
@@ -38,7 +38,7 @@
<param name="frameid" type="string" value="laser_tilt_link" />
<param name="min_ang_degrees" type="double" value="-70.0" />
<param name="max_ang_degrees" type="double" value="70.0" />
- <param name="skip" type="int" value="0" />
+ <param name="skip" type="int" value="1" />
</node>
<!-- imu -->
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/teleop_joystick.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/teleop_joystick.launch 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/teleop_joystick.launch 2009-03-03 02:32:36 UTC (rev 12028)
@@ -15,13 +15,14 @@
<node pkg="teleop_base" type="teleop_base" args="--deadman_no_publish" output="screen"/>
</group>
+<!-- Why is the EKF launched with the joystick?
<param name="robot_pose_ekf/freq" value="30.0"/>
<param name="robot_pose_ekf/sensor_timeout" value="1.0"/>
<param name="robot_pose_ekf/odom_used" value="true"/>
<param name="robot_pose_ekf/imu_used" value="true"/>
<param name="robot_pose_ekf/vo_used" value="false"/>
-<node pkg="robot_pose_ekf" type="robot_pose_ekf" args="robot_pose_ekf"/>
+<node pkg="robot_pose_ekf" type="robot_pose_ekf" args="robot_pose_ekf"/>-->
</launch>
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/collision_map_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/collision_map_display.cpp 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/collision_map_display.cpp 2009-03-03 02:32:36 UTC (rev 12028)
@@ -277,7 +277,7 @@
Ogre::ColourValue color;
uint32_t num_boxes = new_message_->get_boxes_size();
- ROS_INFO("Collision map contains %d boxes.", num_boxes);
+ ROS_DEBUG("Collision map contains %d boxes.", num_boxes);
// If we render points, we don't care about the order
if (render_operation_ == collision_render_ops::CPoints)
Modified: pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp
===================================================================
--- pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp 2009-03-03 02:30:35 UTC (rev 12027)
+++ pkg/trunk/visualization/rviz/src/rviz/displays/planning_display.cpp 2009-03-03 02:32:36 UTC (rev 12028)
@@ -161,7 +161,10 @@
TiXmlDocument doc;
doc.Parse(content.c_str());
if (!doc.RootElement())
+ {
+ ROS_ERROR("Failed to load robot model from description: %s", description_param_.c_str());
return;
+ }
mechanism::Robot descr;
descr.initXml(doc.RootElement());
@@ -214,6 +217,9 @@
void PlanningDisplay::update( float dt )
{
+ if(!kinematic_model_)
+ return;
+
incoming_kinematic_path_message_.lock();
if ( !animating_path_ && new_kinematic_path_ )
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
