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):
rot = np.mat([[1, 0, 0], [0, 1. / np.sqrt(2), -1. / np.sqrt(2)],
[0, 1. / np.sqrt(2), 1. / np.sqrt(2)]])
self.sspace = SpheroidSpace(0.2) #, np.mat([1.0, 0.5, 0.5]).T, 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, 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.
nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
j = np.sqrt(1. / (1. + ny * ny / (nz * nz)))
k = -ny * j / nz
norm_rot = np.mat([[-nx, ny * k - nz * j, 0], [-ny, -nx * k, j],
[-nz, nx * j, k]])
_, norm_quat = PoseConverter.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
rot_angle = np.arctan(-norm_rot[2, 1] / norm_rot[2, 2])
quat_ortho_rot = tf_trans.quaternion_from_euler(
rot_angle + np.pi + rot_gripper, 0.0, 0.0)
norm_quat_ortho = tf_trans.quaternion_multiply(norm_quat,
quat_ortho_rot)
ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos,
norm_quat_ortho)
self.pose_pub.publish(ps_msg)
class SpheroidViz:
def __init__(self):
rot = np.mat([[1, 0, 0], [0, 1./np.sqrt(2), -1./np.sqrt(2)], [0, 1./np.sqrt(2), 1./np.sqrt(2)]])
self.sspace = SpheroidSpace(0.2)#, np.mat([1.0, 0.5, 0.5]).T, 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, 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.
nx, ny, nz = df_du.T.A[0] / np.linalg.norm(df_du)
j = np.sqrt(1./(1.+ny*ny/(nz*nz)))
k = -ny*j/nz
norm_rot = np.mat([[-nx, ny*k - nz*j, 0],
[-ny, -nx*k, j],
[-nz, nx*j, k]])
_, norm_quat = PoseConverter.to_pos_quat(np.mat([0, 0, 0]).T, norm_rot)
rot_angle = np.arctan(-norm_rot[2,1] / norm_rot[2,2])
quat_ortho_rot = tf_trans.quaternion_from_euler(rot_angle + np.pi + rot_gripper, 0.0, 0.0)
norm_quat_ortho = tf_trans.quaternion_multiply(norm_quat, quat_ortho_rot)
ps_msg = PoseConverter.to_pose_stamped_msg("torso_lift_link", pos, norm_quat_ortho)
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)
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"
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 __init__(self):
rot = np.mat([[1, 0, 0], [0, 1./np.sqrt(2), -1./np.sqrt(2)], [0, 1./np.sqrt(2), 1./np.sqrt(2)]])
self.sspace = SpheroidSpace(0.2)#, np.mat([1.0, 0.5, 0.5]).T, 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()
