|
From: <is...@us...> - 2009-01-23 19:57:13
|
Revision: 10072
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=10072&view=rev
Author: isucan
Date: 2009-01-23 19:57:07 +0000 (Fri, 23 Jan 2009)
Log Message:
-----------
waiting for state
Modified Paths:
--------------
pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/run_trajectory_controller.launch
pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/trajectory_controllers.xml
pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups_collision.xml
Modified: pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp
===================================================================
--- pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-23 19:46:31 UTC (rev 10071)
+++ pkg/trunk/motion_planning/plan_kinematic_path/src/plan_kinematic_path.cpp 2009-01-23 19:57:07 UTC (rev 10072)
@@ -421,9 +421,9 @@
plan->loadRobotDescription();
if (plan->loadedRobot())
{
- // plan->waitForState();
-
- sleep(2);
+ sleep(1);
+ plan->waitForState();
+ ROS_INFO("Received robot state");
char test = (argc < 3) ? ' ' : argv[2][0];
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/run_trajectory_controller.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/run_trajectory_controller.launch 2009-01-23 19:46:31 UTC (rev 10071)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/run_trajectory_controller.launch 2009-01-23 19:57:07 UTC (rev 10072)
@@ -1,19 +1,20 @@
+
<launch>
<!-- Arm trajectory controller -->
- <param name="right_arm_trajectory_controller/velocity_scaling_factor" type="double" value="0.25"/>
- <param name="right_arm_trajectory_controller/trajectory_wait_timeout" type="double" value="2.5"/>
+ <param name="right_arm_trajectory_controller/velocity_scaling_factor" type="double" value="1.0"/>
+ <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.01"/>
- <param name="right_arm_trajectory_controller/r_shoulder_lift_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_shoulder_roll_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_elbow_flex_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_forearm_roll_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_wrist_flex_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_wrist_roll_joint/goal_reached_threshold" type="double" value="0.01"/>
- <param name="right_arm_trajectory_controller/r_gripper_l_finger_joint/goal_reached_threshold" type="double" value="0.01"/>
+ <param name="right_arm_trajectory_controller/r_shoulder_pan_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_shoulder_lift_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_shoulder_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_elbow_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_forearm_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_wrist_flex_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_wrist_roll_joint/goal_reached_threshold" type="double" value="0.1"/>
+ <param name="right_arm_trajectory_controller/r_gripper_l_finger_joint/goal_reached_threshold" type="double" value="0.1"/>
- <node pkg="mechanism_control" type="spawner.py" args="$(find arm_cart)/trajectory_controllers.xml" />
+ <node pkg="mechanism_control" type="spawner.py" args="$(find arm_cart)/trajectory_controllers.xml" output="screen"/>
</launch>
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/trajectory_controllers.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/trajectory_controllers.xml 2009-01-23 19:46:31 UTC (rev 10071)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/arm_cart/trajectory_controllers.xml 2009-01-23 19:57:07 UTC (rev 10072)
@@ -13,7 +13,7 @@
</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="15.0" i="0.8" iClamp="100.0" />
+ <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">
Modified: pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-01-23 19:46:31 UTC (rev 10071)
+++ pkg/trunk/robot_descriptions/pr2/pr2_configurations/pr2_alpha/prf.launch 2009-01-23 19:57:07 UTC (rev 10072)
@@ -12,8 +12,10 @@
<include file="$(find pr2_full)/calibrate.launch" />
<!-- Joystick -->
+<!--
<param name="joy/deadzone" value="5000"/>
<node machine="four" pkg="joy" type="joy" respawn="true"/>
+-->
<!-- spacenav -->
<node machine="four" pkg="spacenav_node" type="spacenav_node" />
Modified: pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups_collision.xml
===================================================================
--- pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups_collision.xml 2009-01-23 19:46:31 UTC (rev 10071)
+++ pkg/trunk/robot_descriptions/pr2/pr2_defs/robots/pr2_groups_collision.xml 2009-01-23 19:57:07 UTC (rev 10072)
@@ -36,22 +36,6 @@
torso_lift_link
head_pan_link
head_tilt_link
-
- l_shoulder_pan_link
- l_shoulder_lift_link
- l_upper_arm_roll_link
- l_upper_arm_link
- l_elbow_flex_link
- l_forearm_roll_link
- l_forearm_link
- l_wrist_flex_link
- l_wrist_roll_link
- l_gripper_palm_link
- l_gripper_l_finger_link
- l_gripper_r_finger_link
- l_gripper_l_finger_tip_link
- l_gripper_r_finger_tip_link
-
r_shoulder_pan_link
r_shoulder_lift_link
r_upper_arm_roll_link
@@ -66,7 +50,6 @@
r_gripper_r_finger_link
r_gripper_l_finger_tip_link
r_gripper_r_finger_tip_link
-
</group>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|