ati_force_torque: calculate_offsets.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 from subprocess import call
00004 
00005 import rospy
00006 from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
00007 from ati_force_torque.srv import CalculateSensorOffset
00008 from geometry_msgs.msg import Wrench
00009 
00010 def calculate_sensor_offsets():
00011     
00012     rospy.init_node('calculate_offsets')    
00013     trajectory_pub = rospy.Publisher('/arm/joint_trajectory_controller/command', JointTrajectory, latch=True, queue_size=1)
00014     calculate_offsets_srv = rospy.ServiceProxy('/arm/CalculateOffsets', CalculateSensorOffset)
00015     
00016     joint_names = rospy.get_param('/arm/joint_names')
00017     store_to_file = rospy.get_param('~store_to_file')    
00018     
00019     ##print call('rospack find ati_force_torque', shell=True)
00020     ##call('rosparam dump -v `rospack find ati_force_torque`/config/sensor_offset.yaml /fts/Offset')
00021     
00022     # Posees
00023     poses = [[0.0, 0.0, 1.5707963, 0.0, -1.5707963, 0.0],
00024              [0.0, 0.0, 1.5707963, 0.0, 1.5707963, 0.0]]
00025 
00026     measurement = Wrench()
00027     
00028     for i in range(0,len(poses)):
00029         trajectory = JointTrajectory()
00030         point = JointTrajectoryPoint()
00031         trajectory.header.stamp = rospy.Time.now()
00032         trajectory.joint_names = joint_names
00033         
00034         point.time_from_start = rospy.Duration(5.0)
00035         point.positions = poses[i]
00036         
00037         trajectory.points.append(point)        
00038         trajectory_pub.publish(trajectory)            
00039         rospy.loginfo("Going to position: " + str(poses[i]))
00040                 
00041         rospy.sleep(10.0)
00042         
00043         rospy.loginfo("Calculating offsets.")
00044         ret = calculate_offsets_srv(False)
00045 
00046         measurement.force.x += ret.offset.force.x
00047         measurement.force.y += ret.offset.force.y
00048         measurement.force.z += ret.offset.force.z
00049         measurement.torque.x += ret.offset.torque.x
00050         measurement.torque.y += ret.offset.torque.y
00051         measurement.torque.z += ret.offset.torque.z
00052 
00053     measurement.force.x /= len(poses)
00054     measurement.force.y /= len(poses)
00055     measurement.force.z /= len(poses)
00056     measurement.torque.x /= len(poses)
00057     measurement.torque.y /= len(poses)
00058     measurement.torque.z /= len(poses)
00059 
00060     rospy.set_param('/temp/Offset/force/x', measurement.force.x)
00061     rospy.set_param('/temp/Offset/force/y', measurement.force.y)
00062     rospy.set_param('/temp/Offset/force/z', measurement.force.z)
00063     rospy.set_param('/temp/Offset/torque/x', measurement.torque.x)
00064     rospy.set_param('/temp/Offset/torque/y', measurement.torque.y)
00065     rospy.set_param('/temp/Offset/torque/z', measurement.torque.z)    
00066 
00067     if store_to_file:
00068       call('rosparam dump -v `rospack find ati_force_torque`/config/sensor_offset.yaml /temp/Offset', shell=True)
00069 
00070 
00071 if __name__ == "__main__":
00072     
00073     try:
00074         calculate_sensor_offsets()
00075     except rospy.ROSInterruptException:
00076         pass