Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots:[8919] pkg/trunk
|
From: <hsu...@us...> - 2009-01-06 22:36:11
|
Revision: 8919
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=8919&view=rev
Author: hsujohnhsu
Date: 2009-01-06 22:36:09 +0000 (Tue, 06 Jan 2009)
Log Message:
-----------
* move mech.py spawn from test directory to demo.
* update arm orientation check to use quaternions.
Modified Paths:
--------------
pkg/trunk/demos/arm_gazebo/arm_no_x.launch
pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.py
pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.xml
Modified: pkg/trunk/demos/arm_gazebo/arm_no_x.launch
===================================================================
--- pkg/trunk/demos/arm_gazebo/arm_no_x.launch 2009-01-06 22:34:45 UTC (rev 8918)
+++ pkg/trunk/demos/arm_gazebo/arm_no_x.launch 2009-01-06 22:36:09 UTC (rev 8919)
@@ -15,10 +15,10 @@
<node pkg="gazebo_plugin" type="urdf2factory" args="robotdesc/pr2" respawn="false" output="screen" /> <!-- load default arm controller -->
<!-- start arm controller -->
- <!--node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/controllers.xml" respawn="false" /--> <!-- load default arm controller -->
+ <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" /> <!-- load default arm controller -->
<!-- send arm a command -->
- <!--node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /--> <!-- open gripper .5 radians -->
+ <node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
<!-- for visualization -->
<!-- node pkg="pr2_gui" type="pr2_gui" respawn="false" output="screen" / -->
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.py
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.py 2009-01-06 22:34:45 UTC (rev 8918)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.py 2009-01-06 22:36:09 UTC (rev 8919)
@@ -49,41 +49,52 @@
rostools.update_path('robot_msgs')
rostools.update_path('rostest')
rostools.update_path('rospy')
+rostools.update_path('transformations')
+rostools.update_path('numpy')
-
import sys, unittest
import os, os.path, threading, time
import rospy, rostest
from std_msgs.msg import *
from pr2_mechanism_controllers.msg import *
+from transformations import *
+from numpy import *
-CMD_POS = 0.5
+ARM_JNT_NAMES = ['l_shoulder_pan_joint','l_shoulder_lift_joint', \
+ 'l_upper_arm_roll_joint','l_elbow_flex_joint', \
+ 'l_forearm_roll_joint','l_wrist_flex_joint', \
+ 'l_wrist_roll_joint']
+CMD_POS = [0.5,0.5,0.5,-0.5,0.5,0.5,0.5]
+CMD_VEL = [0,0,0,0,0,0,0]
+GRP_CMD_POS = 0.3
+
TARGET_DURATION = 1.0
-TARGET_TOL = 0.08 #empirical test result john - 20081029
+ROT_TARGET_TOL = 0.01 #empirical test result john - 20090106
+POS_TARGET_TOL = 0.01 #empirical test result john - 20090106
TEST_TIMEOUT = 30.0
# pre-recorded poses for above commands
-TARGET_ARM_TX = 0.70167519
-TARGET_ARM_TY = 0.27016711
-TARGET_ARM_TZ = 0.78997955
-TARGET_ARM_QX = 0.23727666
-TARGET_ARM_QY = -0.18292768
-TARGET_ARM_QZ = 0.19008174
-TARGET_ARM_QW = 0.93493646
-TARGET_GRIPPER_TX = 0.76451604
-TARGET_GRIPPER_TY = 0.29929084
-TARGET_GRIPPER_TZ = 0.82737631
-TARGET_GRIPPER_QX = 0.22177592
-TARGET_GRIPPER_QY = -0.20146688
-TARGET_GRIPPER_QZ = 0.26480912
-TARGET_GRIPPER_QW = 0.91654743
-
+TARGET_ARM_TX = 0.703305819503
+TARGET_ARM_TY = 0.270579779085
+TARGET_ARM_TZ = 0.789835186655
+TARGET_ARM_QX = -0.23728153595
+TARGET_ARM_QY = 0.182723242802
+TARGET_ARM_QZ = -0.190178621726
+TARGET_ARM_QW = -0.934955496842
+TARGET_GRIPPER_TX = 0.765601972524
+TARGET_GRIPPER_TY = 0.299927644187
+TARGET_GRIPPER_TZ = 0.827281715659
+TARGET_GRIPPER_QX = -0.201821946334
+TARGET_GRIPPER_QY = 0.221139617347
+TARGET_GRIPPER_QZ = -0.350190747038
+TARGET_GRIPPER_QW = -0.887542456622
class ArmTest(unittest.TestCase):
def __init__(self, *args):
super(ArmTest, self).__init__(*args)
- self.success = False
+ self.arm_success = False
+ self.grp_success = False
self.reached_target_arm = False
self.reached_target_gripper = False
self.duration_start_gripper = 0
@@ -108,53 +119,83 @@
def gripperP3dInput(self, p3d):
i = 0
- error = abs(p3d.pos.position.x - TARGET_GRIPPER_TX) + \
- abs(p3d.pos.position.y - TARGET_GRIPPER_TY) + \
- abs(p3d.pos.position.z - TARGET_GRIPPER_TZ) + \
- abs(p3d.pos.orientation.x - TARGET_GRIPPER_QX) + \
- abs(p3d.pos.orientation.y - TARGET_GRIPPER_QY) + \
- abs(p3d.pos.orientation.z - TARGET_GRIPPER_QZ) + \
- abs(p3d.pos.orientation.w - TARGET_GRIPPER_QW)
- print " gripper Error: " + str(error)
+ pos_error = abs(p3d.pos.position.x - TARGET_GRIPPER_TX) + \
+ abs(p3d.pos.position.y - TARGET_GRIPPER_TY) + \
+ abs(p3d.pos.position.z - TARGET_GRIPPER_TZ)
+
+ target_rm = rotation_matrix_from_quaternion([TARGET_GRIPPER_QX \
+ ,TARGET_GRIPPER_QY \
+ ,TARGET_GRIPPER_QZ \
+ ,TARGET_GRIPPER_QW])
+
+ p3d_q = [p3d.pos.orientation.x \
+ ,p3d.pos.orientation.y \
+ ,p3d.pos.orientation.z \
+ ,p3d.pos.orientation.w]
+
+ target_q_inv = quaternion_from_rotation_matrix( linalg.inv(target_rm) )
+
+ rot_euler = euler_from_quaternion( quaternion_multiply(p3d_q, target_q_inv) )
+
+ rot_error = abs( rot_euler[0] ) + \
+ abs( rot_euler[1] ) + \
+ abs( rot_euler[2] )
+
+ print " gripper Error pos: " + str(pos_error) + " rot: " + str(rot_error)
#self.printP3D(p3d)
# has to reach target vw and maintain target vw for a duration of TARGET_DURATION seconds
if self.reached_target_gripper:
print " gripper duration: " + str(time.time() - self.duration_start_gripper)
- if error < TARGET_TOL:
+ if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
if time.time() - self.duration_start_gripper > TARGET_DURATION:
- self.success = True
+ self.grp_success = True
else:
# failed to maintain target vw, reset duration
- self.success = False
+ self.grp_success = False
self.reached_target_gripper = False
else:
- if error < TARGET_TOL:
+ if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
self.reached_target_gripper = True
self.duration_start_gripper = time.time()
def armP3dInput(self, p3d):
i = 0
- error = abs(p3d.pos.position.x - TARGET_ARM_TX) + \
- abs(p3d.pos.position.y - TARGET_ARM_TY) + \
- abs(p3d.pos.position.z - TARGET_ARM_TZ) + \
- abs(p3d.pos.orientation.x - TARGET_ARM_QX) + \
- abs(p3d.pos.orientation.y - TARGET_ARM_QY) + \
- abs(p3d.pos.orientation.z - TARGET_ARM_QZ) + \
- abs(p3d.pos.orientation.w - TARGET_ARM_QW)
- print " arm Error: " + str(error)
+ pos_error = abs(p3d.pos.position.x - TARGET_ARM_TX) + \
+ abs(p3d.pos.position.y - TARGET_ARM_TY) + \
+ abs(p3d.pos.position.z - TARGET_ARM_TZ)
+
+ target_rm = rotation_matrix_from_quaternion([TARGET_ARM_QX \
+ ,TARGET_ARM_QY \
+ ,TARGET_ARM_QZ \
+ ,TARGET_ARM_QW])
+
+ p3d_q = [p3d.pos.orientation.x \
+ ,p3d.pos.orientation.y \
+ ,p3d.pos.orientation.z \
+ ,p3d.pos.orientation.w]
+
+ target_q_inv = quaternion_from_rotation_matrix( linalg.inv(target_rm) )
+
+ rot_euler = euler_from_quaternion( quaternion_multiply(p3d_q, target_q_inv) )
+
+ rot_error = abs( rot_euler[0] ) + \
+ abs( rot_euler[1] ) + \
+ abs( rot_euler[2] )
+
+ print " arm Error pos: " + str(pos_error) + " rot: " + str(rot_error)
#self.printP3D(p3d)
# has to reach target vw and maintain target vw for a duration of TARGET_DURATION seconds
if self.reached_target_arm:
print " arm duration: " + str(time.time() - self.duration_start_arm)
- if error < TARGET_TOL:
+ if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
if time.time() - self.duration_start_arm > TARGET_DURATION:
- self.success = True
+ self.arm_success = True
else:
# failed to maintain target vw, reset duration
- self.success = False
+ self.arm_success = False
self.reached_target_arm = False
else:
- if error < TARGET_TOL:
+ if rot_error < ROT_TARGET_TOL and pos_error < POS_TARGET_TOL:
self.reached_target_arm = True
self.duration_start_arm = time.time()
@@ -166,13 +207,11 @@
rospy.Subscriber("l_gripper_l_finger_pose_ground_truth", PoseWithRatesStamped, self.gripperP3dInput)
rospy.init_node(NAME, anonymous=True)
timeout_t = time.time() + TEST_TIMEOUT
- while not rospy.is_shutdown() and not self.success and time.time() < timeout_t:
- pub_arm.publish(JointPosCmd(['l_shoulder_pan_joint','l_shoulder_lift_joint','l_upper_arm_roll_joint','l_elbow_flex_joint','l_forearm_roll_joint','l_wrist_flex_joint','l_wrist_roll_joint'],[CMD_POS,CMD_POS,CMD_POS,CMD_POS,CMD_POS,CMD_POS,CMD_POS],[0,0,0,0,0,0,0],0))
- pub_gripper.publish(Float64(CMD_POS))
- time.sleep(0.5)
- self.assert_(self.success)
-
+ while not rospy.is_shutdown() and (not self.arm_success or not self.grp_success) and time.time() < timeout_t:
+ pub_arm.publish(JointPosCmd(ARM_JNT_NAMES,CMD_POS,CMD_VEL,0))
+ pub_gripper.publish(Float64(GRP_CMD_POS))
+ time.sleep(1.0)
+ self.assert_(self.arm_success and self.grp_success)
if __name__ == '__main__':
rostest.run(PKG, sys.argv[0], ArmTest, sys.argv) #, text_mode=True)
-
Modified: pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.xml
===================================================================
--- pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.xml 2009-01-06 22:34:45 UTC (rev 8918)
+++ pkg/trunk/drivers/simulator/gazebo_plugin/test/test_arm.xml 2009-01-06 22:36:09 UTC (rev 8919)
@@ -4,13 +4,6 @@
<!-- send single_link.xml to param server -->
<include file="$(find arm_gazebo)/arm_no_x.launch" />
- <!-- start arm controller -->
- <node pkg="mechanism_control" type="mech.py" args="sp $(find arm_gazebo)/l_arm_default_controller.xml" respawn="false" /> <!-- load default arm controller -->
-
- <!-- send arm a command -->
- <node pkg="robot_mechanism_controllers" type="control.py" args="set l_gripper_controller 0.5" respawn="false" output="screen" /> <!-- open gripper .5 radians -->
-
-
<!--<node pkg="gazebo_plugin" type="groundtruthtransform" args="" respawn="true" />-->
<test test-name="gazebo_plugin_test_arm" pkg="gazebo_plugin" type="test_arm.py" />
</launch>
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
