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 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
class SpheroidViz:
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()
def publish_vector(self, m_id):
new_m = copy.deepcopy(self.m)
new_m.id = m_id
u, v, p = 1.0, np.random.uniform(0, np.pi), np.random.uniform(
0, 2 * np.pi)
pos = self.sspace.spheroidal_to_cart((u, v, p))
new_m.points.append(Point(*pos))
df_du = self.sspace.partial_u((u, v, p))
df_du *= 0.1 / np.linalg.norm(df_du)
new_m.points.append(Point(*(pos + df_du)))
self.ma.markers.append(new_m)
self.vis_pub.publish(self.ma)
rot_gripper = np.pi / 4.
pos, rot = self.sspace.spheroidal_to_pose((u, v, p), rot_gripper)
ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, rot)
self.pose_pub.publish(ps_msg)
class SpheroidViz:
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()
def publish_vector(self, m_id):
new_m = copy.deepcopy(self.m)
new_m.id = m_id
u, v, p = 1.0, np.random.uniform(0, np.pi), np.random.uniform(0, 2 * np.pi)
pos = self.sspace.spheroidal_to_cart((u, v, p))
new_m.points.append(Point(*pos))
df_du = self.sspace.partial_u((u, v, p))
df_du *= 0.1 / np.linalg.norm(df_du)
new_m.points.append(Point(*(pos+df_du)))
self.ma.markers.append(new_m)
self.vis_pub.publish(self.ma)
rot_gripper = np.pi/4.
pos, rot = self.sspace.spheroidal_to_pose((u,v,p), rot_gripper)
ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, rot)
self.pose_pub.publish(ps_msg)
def main():
rospy.init_node("pr2_arm_test")
arm = sys.argv[1]
jnt_arm = create_pr2_arm(arm, arm_type=PR2ArmJointTrajectory)
kin = jnt_arm.kinematics
ellipse_rot = np.mat([[-1., 0., 0.], [0., -1., 0.], [0., 0., 1.]])
sspace = SpheroidSpace(0.15, np.mat([0.78, -0.18, 0.1]).T, ellipse_rot)
uvp = np.array([1.0, np.pi/2, 0.0])
uvp_delta = np.array([0.0, 0.6, 0.6])
pos, rot = sspace.spheroidal_to_pose(uvp + uvp_delta)
print pos, rot
#q_ik = kin.IK_search(pos, rot)
q_ik = kin.IK(pos, rot, jnt_arm.get_joint_angles())
if q_ik is not None:
jnt_arm.set_ep(q_ik, 5)
else:
print "IK failure"
