Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[20707] pkg/trunk
|
From: <stu...@us...> - 2009-08-04 21:52:04
|
Revision: 20707
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=20707&view=rev
Author: stuglaser
Date: 2009-08-04 21:51:56 +0000 (Tue, 04 Aug 2009)
Log Message:
-----------
Updated tuckarm.py for the controller spawning changes.
Modified Paths:
--------------
pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_joint_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_joint_trajectory_controller.xml
pkg/trunk/stacks/pr2/pr2_default_controllers/scripts/tuckarm.py
Modified: pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp
===================================================================
--- pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-08-04 21:51:48 UTC (rev 20706)
+++ pkg/trunk/controllers/pr2_mechanism_controllers/src/joint_trajectory_controller.cpp 2009-08-04 21:51:56 UTC (rev 20707)
@@ -88,9 +88,15 @@
TiXmlDocument xml_doc;
xml_doc.Parse(xml_string.c_str());
+ if (xml_doc.Error())
+ {
+ ROS_ERROR("Error when parsing XML: %d (%d,%d) %s\n%s", xml_doc.ErrorId(),
+ xml_doc.ErrorRow(), xml_doc.ErrorCol(), xml_doc.ErrorDesc(), xml_string.c_str());
+ return false;
+ }
TiXmlElement *xml = xml_doc.FirstChildElement("controller");
if (!xml){
- ROS_ERROR("could not parse xml %s",xml_string.c_str());
+ ROS_ERROR("could not parse xml: %s\n %s", xml_doc.ErrorDesc(),xml_string.c_str());
return false;
}
Modified: pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_joint_trajectory_controller.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_joint_trajectory_controller.xml 2009-08-04 21:51:48 UTC (rev 20706)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/l_arm_joint_trajectory_controller.xml 2009-08-04 21:51:56 UTC (rev 20707)
@@ -1,42 +1,40 @@
<?xml version="1.0"?>
-<controllers>
- <controller name="l_arm_joint_trajectory_controller" type="JointTrajectoryController">
- <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>
- <trajectory interpolation="cubic" />
+<controller name="l_arm_joint_trajectory_controller" type="JointTrajectoryController">
+ <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>
-</controllers>
+ <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>
+ <trajectory interpolation="cubic" />
+</controller>
Modified: pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_joint_trajectory_controller.xml
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_joint_trajectory_controller.xml 2009-08-04 21:51:48 UTC (rev 20706)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/r_arm_joint_trajectory_controller.xml 2009-08-04 21:51:56 UTC (rev 20707)
@@ -1,42 +1,40 @@
<?xml version="1.0"?>
-<controllers>
- <controller name="r_arm_joint_trajectory_controller" type="JointTrajectoryController">
- <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="100.0" iClamp="10000.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="100.0" iClamp="10000.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="100.0" iClamp="10000.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="100.0" iClamp="10000.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="100.0" iClamp="10000.0" />
- </joint>
- </controller>
- <controller name="r_wrist_flex_controller" type="JointPDController">
- <joint name="r_wrist_flex_joint">
- <pid p="20.0" i="100.0" d="0.5" iClamp="10000.0" />
- </joint>
- </controller>
- <controller name="r_wrist_roll_controller" type="JointPDController">
- <joint name="r_wrist_roll_joint">
- <pid p="10" i="100.0" d="0.5" iClamp="10000.0" />
- </joint>
- </controller>
- <trajectory interpolation="cubic" />
+<controller name="r_arm_joint_trajectory_controller" type="JointTrajectoryController">
+ <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="100.0" iClamp="10000.0" />
+ </joint>
</controller>
-</controllers>
+ <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="100.0" iClamp="10000.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="100.0" iClamp="10000.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="100.0" iClamp="10000.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="100.0" iClamp="10000.0" />
+ </joint>
+ </controller>
+ <controller name="r_wrist_flex_controller" type="JointPDController">
+ <joint name="r_wrist_flex_joint">
+ <pid p="20.0" i="100.0" d="0.5" iClamp="10000.0" />
+ </joint>
+ </controller>
+ <controller name="r_wrist_roll_controller" type="JointPDController">
+ <joint name="r_wrist_roll_joint">
+ <pid p="10" i="100.0" d="0.5" iClamp="10000.0" />
+ </joint>
+ </controller>
+ <trajectory interpolation="cubic" />
+</controller>
Modified: pkg/trunk/stacks/pr2/pr2_default_controllers/scripts/tuckarm.py
===================================================================
--- pkg/trunk/stacks/pr2/pr2_default_controllers/scripts/tuckarm.py 2009-08-04 21:51:48 UTC (rev 20706)
+++ pkg/trunk/stacks/pr2/pr2_default_controllers/scripts/tuckarm.py 2009-08-04 21:51:56 UTC (rev 20707)
@@ -64,6 +64,7 @@
USAGE = 'tuckarm.py <arms> ; <arms> is \'(r)ight\', \'(l)eft\', or \'(b)oth\' arms'
def set_params_right():
+ rospy.set_param("r_arm_joint_trajectory_controller/type", "JointTrajectoryController")
rospy.set_param("r_arm_joint_trajectory_controller/velocity_scaling_factor", 0.5)
rospy.set_param("r_arm_joint_trajectory_controller/trajectory_wait_timeout", 0.25)
@@ -76,6 +77,7 @@
rospy.set_param("r_arm_joint_trajectory_controller/r_wrist_roll_joint/goal_reached_threshold", 0.1)
def set_params_left():
+ rospy.set_param("l_arm_joint_trajectory_controller/type", "JointTrajectoryController")
rospy.set_param("l_arm_joint_trajectory_controller/velocity_scaling_factor", 0.5)
rospy.set_param("l_arm_joint_trajectory_controller/trajectory_wait_timeout", 0.25)
@@ -103,13 +105,16 @@
xml_for_left = open(os.path.join(path, 'l_arm_joint_trajectory_controller.xml'))
xml_for_right = open(os.path.join(path, 'r_arm_joint_trajectory_controller.xml'))
-
+ set_params_left()
+ rospy.set_param('l_arm_joint_trajectory_controller/xml', xml_for_left.read())
+ set_params_right()
+ rospy.set_param('r_arm_joint_trajectory_controller/xml', xml_for_right.read())
+
controllers = []
try:
if side == 'l' or side == 'left':
# tuck traj for left arm
- set_params_left()
- mechanism.spawn_controller(xml_for_left.read(), 1)
+ mechanism.spawn_controller('l_arm_joint_trajectory_controller', True)
controllers.append('l_arm_joint_trajectory_controller')
positions = [[0.4,0.0,0.0,-2.25,0.0,0.0,0.0], [0.0,1.57,1.57,-2.25,0.0,0.0,0.0]]
@@ -119,8 +124,7 @@
elif side == 'r' or side == 'right':
# tuck traj for right arm
- set_params_right()
- resp = mechanism.spawn_controller(xml_for_right.read(), 1)
+ mechanism.spawn_controller('r_arm_joint_trajectory_controller', True)
controllers.append('r_arm_joint_trajectory_controller')
positions = [[-0.4,0.0,0.0,-1.57,0.0,0.0,0.0], [0.0,1.57,-1.57,-1.57,0.0,0.0,0.0]]
go('r', positions)
@@ -130,12 +134,9 @@
elif side == 'b' or side == 'both':
# Both arms
# Holds left arm up at shoulder lift
- set_params_left()
- resp = mechanism.spawn_controller(xml_for_left.read(), 1)
+ mechanism.spawn_controller('r_arm_joint_trajectory_controller', True)
+ mechanism.spawn_controller('l_arm_joint_trajectory_controller', True)
- set_params_right()
- resp = mechanism.spawn_controller(xml_for_right.read(), 1)
-
controllers.append('r_arm_joint_trajectory_controller')
controllers.append('l_arm_joint_trajectory_controller')
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
