def main():
rospy.init_node("ellipsoidal_ik")
sspace = SpheroidSpace(0.2, np.mat([0.78, -0.28, 0.3]).T)
kin = PR2ArmKinematics('r')
jarm = PR2ArmJointTrajectory('r', kin)
while not rospy.is_shutdown():
u, v, p = 1, np.random.uniform(0,
np.pi), np.random.uniform(0, 2 * np.pi)
pos, rot = sspace.spheroidal_to_pose((u, v, p))
q = kin.IK(pos, rot)
if not q is None:
print q
#print np.mod(q, 2 * np.pi)
rospy.sleep(0.1)
#jarm.set_ep([0.1]*7, 15)
return
jfv = SpheroidViz()
i = 0
while not rospy.is_shutdown():
jfv.publish_vector(i)
i += 1
rospy.sleep(0.5)
return
def __init__(self):
ellipse_rot = np.mat([[-1., 0., 0.], [0., -1., 0.], [0., 0., 1.]])
self.sspace = SpheroidSpace(0.15,
np.mat([0.78, -0.18, 0.1]).T, ellipse_rot)
self.colors = [ColorRGBA(1., 0., 0., 1.), ColorRGBA(0., 1., 0., 1.)]
self.vis_pub = rospy.Publisher("/force_torque_markers_array",
MarkerArray)
self.pose_pub = rospy.Publisher("/spheroid_poses", PoseStamped)
m = Marker()
m.header.frame_id = "torso_lift_link"
m.header.stamp = rospy.Time()
m.ns = "force_torque"
m.id = 0
m.type = Marker.ARROW
m.action = Marker.ADD
#m.points.append(Point(0, 0, 0))
m.scale = Vector3(0.01, 0.01, 0.01)
m.color = self.colors[0]
#self.vis_pub.publish(m)
self.m = m
self.ma = MarkerArray()
