ellipsoidal_viz.py
Go to the documentation of this file.
00001 #! /usr/bin/python
00002 
00003 import sys
00004 import numpy as np
00005 import copy
00006 
00007 import roslib
00008 roslib.load_manifest("hrl_pr2_arms")
00009 roslib.load_manifest("kelsey_sandbox")
00010 import rospy
00011 from visualization_msgs.msg import Marker, MarkerArray
00012 from geometry_msgs.msg import Point, Quaternion, Vector3, PoseStamped
00013 from std_msgs.msg import ColorRGBA
00014 from sensor_msgs.msg import JointState
00015 import tf.transformations as tf_trans
00016 from hrl_generic_arms.pose_converter import PoseConverter
00017 
00018 from spheroid_space import SpheroidSpace
00019 
00020 JOINT_STATE_INDS_R = [17, 18, 16, 20, 19, 21, 22]
00021 JOINT_STATE_INDS_L = [29, 30, 28, 32, 31, 33, 34]
00022 
00023 class SpheroidViz:
00024     def __init__(self):
00025         ellipse_rot = np.mat([[-1., 0., 0.], [0., -1., 0.], [0., 0., 1.]])
00026         self.sspace = SpheroidSpace(0.15, np.mat([0.78, -0.18, 0.1]).T, ellipse_rot)
00027         self.colors = [ColorRGBA(1., 0., 0., 1.), ColorRGBA(0., 1., 0., 1.)]
00028         self.vis_pub = rospy.Publisher("/force_torque_markers_array", MarkerArray)
00029         self.pose_pub = rospy.Publisher("/spheroid_poses", PoseStamped)
00030 
00031         m = Marker()
00032         m.header.frame_id = "torso_lift_link"
00033         m.header.stamp = rospy.Time()
00034         m.ns = "force_torque"
00035         m.id = 0
00036         m.type = Marker.ARROW
00037         m.action = Marker.ADD
00038         #m.points.append(Point(0, 0, 0))
00039         m.scale = Vector3(0.01, 0.01, 0.01)
00040         m.color = self.colors[0]
00041         #self.vis_pub.publish(m)
00042         self.m = m
00043         
00044         self.ma = MarkerArray()
00045 
00046     def publish_vector(self, m_id):
00047         new_m = copy.deepcopy(self.m)
00048         new_m.id = m_id
00049         u, v, p = 1.0, np.random.uniform(0, np.pi), np.random.uniform(0, 2 * np.pi)
00050         pos = self.sspace.spheroidal_to_cart((u, v, p))
00051         new_m.points.append(Point(*pos))
00052 
00053         df_du = self.sspace.partial_u((u, v, p))
00054         df_du *= 0.1 / np.linalg.norm(df_du)
00055         new_m.points.append(Point(*(pos+df_du)))
00056         
00057         self.ma.markers.append(new_m)
00058         self.vis_pub.publish(self.ma)
00059 
00060         rot_gripper = np.pi/4.
00061 
00062         pos, rot = self.sspace.spheroidal_to_pose((u,v,p), rot_gripper)
00063 
00064         ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, rot)
00065         self.pose_pub.publish(ps_msg)
00066 
00067 
00068 def main():
00069     rospy.init_node("test_markers")
00070 
00071     jfv = SpheroidViz()
00072     i = 0
00073     while not rospy.is_shutdown():
00074         jfv.publish_vector(i)
00075         i += 1
00076         rospy.sleep(0.1)
00077     return
00078 
00079 if __name__ == "__main__":
00080     main()


kelsey_sandbox
Author(s): kelsey
autogenerated on Wed Nov 27 2013 11:52:03