kinematics_estimation.py
Go to the documentation of this file.
00001 
00002 import roslib; roslib.load_manifest('modeling_forces')
00003 import rospy
00004 
00005 from geometry_msgs.msg import Point32
00006 from articulation_msgs.msg import ModelMsg
00007 from articulation_msgs.msg import TrackMsg
00008 from geometry_msgs.msg import Pose, Point, Quaternion
00009 
00010 from threading import RLock
00011 import numpy as np
00012 import time
00013 
00014 def model_cb(data, kin_dict):
00015     print 'model_cb called'
00016     if data.name == 'rotational':
00017         for p in data.params:
00018             if p.name == 'rot_center.x':
00019                 cx = p.value
00020             if p.name == 'rot_center.y':
00021                 cy = p.value
00022             if p.name == 'rot_center.z':
00023                 cz = p.value
00024             if p.name == 'rot_radius':
00025                 r = p.value
00026         kin_dict['typ'] = 'rotational'
00027         kin_dict['cx'] = cx
00028         kin_dict['cy'] = cy
00029         kin_dict['cz'] = cz
00030         kin_dict['r'] = r
00031 
00032     if data.name == 'prismatic':
00033         for p in data.params:
00034             if p.name == 'prismatic_dir.x':
00035                 dx = p.value
00036             if p.name == 'prismatic_dir.y':
00037                 dy = p.value
00038             if p.name == 'prismatic_dir.z':
00039                 dz = p.value
00040         kin_dict['typ'] = 'prismatic'
00041         kin_dict['direc'] = [dx, dy, dz]
00042 
00043     kin_dict['got_model'] = True
00044 
00045 
00046 ##
00047 # fit either a circle or a linear model.
00048 # @param pts - 3xN np matrix of points.
00049 # @return dictionary with kinematic params. (see model_cb)
00050 def fit(pts, tup):
00051     kin_dict, track_pub = tup
00052     track_msg = TrackMsg()
00053     track_msg.header.stamp = rospy.get_rostime()
00054     track_msg.header.frame_id = '/'
00055     track_msg.track_type = TrackMsg.TRACK_POSITION_ONLY
00056     kin_dict['got_model'] = False
00057     for pt in pts.T:
00058         p = pt.A1.tolist()
00059         pose = Pose(Point(0, 0, 0), Quaternion(0, 0, 0, 1))
00060         pose.position.x = p[0]
00061         pose.position.y = p[1]
00062         pose.position.z = p[2]
00063         track_msg.pose.append(pose)
00064 
00065     rospy.sleep(0.1)
00066     track_pub.publish(track_msg)
00067     while kin_dict['got_model'] == False:
00068         rospy.sleep(0.01)
00069     return kin_dict
00070 
00071 def init_ros_node():
00072     rospy.init_node('kinematics_estimation_sturm')
00073     track_pub = rospy.Publisher('track', TrackMsg)
00074     kin_dict = {}
00075     track_msg = TrackMsg()
00076     rospy.Subscriber('model', ModelMsg, model_cb,
00077                      kin_dict)
00078     return kin_dict, track_pub
00079 
00080 
00081 if __name__ == '__main__':
00082     init_ros_node()
00083     rospy.spin()
00084 


2010_biorob_everyday_mechanics
Author(s): Advait Jain, Hai Nguyen, Charles C. Kemp (Healthcare Robotics Lab, Georgia Tech)
autogenerated on Wed Nov 27 2013 11:58:43