def _append_row(self, seq, ts, gt_tf, odo_tf, slam_tf, rel_tf, trans_err,
rot_err, tot_err):
"""
Append a row to the CSV file with the poses and errors
:param seq: (int) Sequence number of the Ground Truth Pose message.
:param ts: (rospy.Time) Timestamp of the Ground Truth Pose message.
:param gt_tf: (np.ndarray) GT:map->base transform matrix.
:param odo_tf: (np.ndarray) ODOM:map->base (= odom->base) transform matrix.
:param slam_tf: (np.ndarray) SLAM:map->base transform matrix.
:param rel_tf: (np.ndarray) Relative transform between GT:base->SLAM:Base transform matrix.
:param trans_err: (float) Step translational error.
:param rot_err: (float) Step rotational error.
:param tot_err: (float) Step total error.
:return: None
"""
if not self._log_error:
return
_, _, gt_rot, gt_tra, _ = decompose_matrix(gt_tf)
gt_p_x, gt_p_y, gt_p_z = gt_tra
gt_r_x, gt_r_y, gt_r_z = gt_rot
_, _, od_rot, od_tra, _ = decompose_matrix(odo_tf)
od_p_x, od_p_y, od_p_z = od_tra
od_r_x, od_r_y, od_r_z = od_rot
_, _, sl_rot, sl_tra, _ = decompose_matrix(slam_tf)
sl_p_x, sl_p_y, sl_p_z = sl_tra
sl_r_x, sl_r_y, sl_r_z = sl_rot
_, _, rl_rot, rl_tra, _ = decompose_matrix(rel_tf)
rl_p_x, rl_p_y, rl_p_z = rl_tra
rl_r_x, rl_r_y, rl_r_z = rl_rot
ts_time = time.localtime(ts.secs)
ts_yy = ts_time.tm_year
ts_mm = ts_time.tm_mon
ts_dd = ts_time.tm_mday
ts_h = ts_time.tm_hour
ts_m = ts_time.tm_min
ts_s = ts_time.tm_sec
ts_ns = ts.nsecs
row = [
seq, ts_yy, ts_mm, ts_dd, ts_h, ts_m, ts_s, ts_ns, gt_p_x, gt_p_y,
gt_p_z, gt_r_x, gt_r_y, gt_r_z, od_p_x, od_p_y, od_p_z, od_r_x,
od_r_y, od_r_z, sl_p_x, sl_p_y, sl_p_z, sl_r_x, sl_r_y, sl_r_z,
rl_p_x, rl_p_y, rl_p_z, rl_r_x, rl_r_y, rl_r_z, trans_err,
self._cum_trans_err, rot_err, self._cum_rot_err, tot_err,
self._cum_tot_err
]
row_str = self._delim.join([str(x) for x in row])
row_str += self._newline
rospy.loginfo("Adding pose with seq. {} to file.".format(seq))
with open(self._current_err_file, 'a') as f:
f.write(row_str)
def transform_tf_to_msg(tf):
_, _, rotation, translation, _ = tft.decompose_matrix(tf)
translation = transform_tf_to_vector3_msg(translation)
rotation = transform_tf_to_quaternion_msg(rotation)
return Transform(translation, rotation)
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
now = rospy.Time.now()
self.listener.waitForTransform(self.odom_frame,self.body_frame, now, rospy.Duration(1.0))
(trans,rot) = self.listener.lookupTransform(self.odom_frame,self.body_frame, now)
new_odom = mat(self.listener.fromTranslationRotation(trans,rot))
# print "================================================"
# print new_odom
# print self.old_odom
odom = new_odom * inv(self.old_odom)
self.old_odom = new_odom
self.lock.acquire()
self.predict(odom)
theta = self.X[2,0]
pose_mat = mat([[cos(theta), -sin(theta), 0, self.X[0,0]],
[sin(theta), cos(theta), 0, self.X[1,0]],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1],
]);
correction_mat = inv(new_odom) * pose_mat
self.lock.release()
scale, shear, angles, trans, persp = decompose_matrix(correction_mat)
self.broadcaster.sendTransform(trans,
quaternion_from_euler(*angles),now, self.odom_frame,self.target_frame)
self.publish(now)
rate.sleep()
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
now = rospy.Time.now()
lasttf = now #rospy.Time(0)
self.listener.waitForTransform(self.odom_frame, self.body_frame,
now, rospy.Duration(1.0))
(trans,
rot) = self.listener.lookupTransform(self.odom_frame,
self.body_frame, lasttf)
new_odom = mat(self.listener.fromTranslationRotation(trans, rot))
euler = tf.transformations.euler_from_quaternion(rot)
deltar = euler[2] - self.lastr
deltat = map(lambda x, y: x - y, trans, self.lastt)
self.lastt = trans
self.lastr = euler[2]
self.lock.acquire()
self.predict(deltat, deltar)
theta = self.X[2, 0]
pose_mat = mat([
[cos(theta), -sin(theta), 0, self.X[0, 0]],
[sin(theta), cos(theta), 0, self.X[1, 0]],
[0, 0, 1, 0],
[0, 0, 0, 1],
])
correction_mat = pose_mat * inv(new_odom)
self.lock.release()
scale, shear, angles, trans, persp = decompose_matrix(
correction_mat)
self.broadcaster.sendTransform(trans,
quaternion_from_euler(*angles), now,
self.odom_frame, self.target_frame)
self.publish(now)
rate.sleep()
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
now = rospy.Time.now()
self.listener.waitForTransform(self.odom_frame,self.body_frame, now, rospy.Duration(1.0))
(trans,rot) = self.listener.lookupTransform(self.odom_frame,self.body_frame, now)
new_odom = mat(self.listener.fromTranslationRotation(trans,rot))
# print "================================================"
# print new_odom
# print self.old_odom
odom = new_odom * inv(self.old_odom)
self.old_odom = new_odom
self.lock.acquire()
self.predict(odom)
theta = self.X[2,0]
pose_mat = mat([[cos(theta), -sin(theta), 0, self.X[0,0]],
[sin(theta), cos(theta), 0, self.X[1,0]],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1],
]);
correction_mat = inv(new_odom) * pose_mat
self.lock.release()
scale, shear, angles, trans, persp = decompose_matrix(correction_mat)
self.broadcaster.sendTransform(trans,
quaternion_from_euler(*angles),now, self.odom_frame,self.target_frame)
self.publish(now)
rate.sleep()
def run(self):
rate = rospy.Rate(10)
while not rospy.is_shutdown():
now = rospy.Time.now()
lasttf = now #rospy.Time(0)
self.listener.waitForTransform(self.odom_frame,self.body_frame, now, rospy.Duration(1.0))
(trans,rot) = self.listener.lookupTransform(self.odom_frame,self.body_frame, lasttf)
new_odom = mat(self.listener.fromTranslationRotation(trans,rot))
euler = tf.transformations.euler_from_quaternion(rot);
deltar = euler[2]-self.lastr;
deltat = map(lambda x, y: x-y, trans, self.lastt)
self.lastt = trans;
self.lastr = euler[2];
self.lock.acquire()
self.predict(deltat, deltar);
theta = self.X[2,0]
pose_mat = mat([[cos(theta), -sin(theta), 0, self.X[0,0]],
[sin(theta), cos(theta), 0, self.X[1,0]],
[ 0, 0, 1, 0],
[ 0, 0, 0, 1],
]);
correction_mat = pose_mat * inv(new_odom);
self.lock.release()
scale, shear, angles, trans, persp = decompose_matrix(correction_mat)
self.broadcaster.sendTransform(trans,
quaternion_from_euler(*angles),now, self.odom_frame, self.target_frame)
self.publish(now)
rate.sleep()
def pose_callback(self, msg):
print "pose cb"
trans = [msg.pose.position.x, msg.pose.position.y, msg.pose.position.z]
rot = [
msg.pose.orientation.x, msg.pose.orientation.y,
msg.pose.orientation.z, msg.pose.orientation.w
]
pose_matrix = tr.compose_matrix(angles=tr.euler_from_quaternion(rot),
translate=trans)
# Transfer to the frame of the robot
# pose_matrix = np.dot(self.config, pose_matrix)
if self.previous_tf is None: #if first callback
odo_trans = trans
odo_rot = rot
odo = tr.compose_matrix(angles=tr.euler_from_quaternion(odo_rot),
translate=odo_trans)
self.origin = trans
else:
# calculate limit matrix
if enable_filter:
limit_dist = limit_speed * (msg.header.stamp.to_nsec() -
self.previous_time.to_nsec())
print(msg.header.stamp.to_nsec() -
self.previous_time.to_nsec())
scale, shear, angles, prev_trans, persp = tr.decompose_matrix(
self.previous_tf)
moved_vec = [
msg.pose.position.x - prev_trans[0],
msg.pose.position.y - prev_trans[1],
msg.pose.position.z - prev_trans[2]
]
moved_dist = np.linalg.norm(moved_vec)
if moved_dist > limit_dist:
#discard this pose
print "move too much"
return
odo = np.dot(tr.inverse_matrix(self.previous_tf), pose_matrix)
odo_trans = tf.transformations.translation_from_matrix(odo)
odo_rot = tf.transformations.quaternion_from_matrix(odo)
self.previous_time = msg.header.stamp
self.previous_tf = pose_matrix
print "x: ", trans[0] - self.origin[0], "y: ", trans[1] - self.origin[
1], "z: ", trans[2] - self.origin[2]
robot_odo = PoseStamped()
robot_odo.header.stamp = msg.header.stamp
robot_odo.pose.position.x = odo_trans[0]
robot_odo.pose.position.y = odo_trans[1]
robot_odo.pose.position.z = odo_trans[2]
robot_odo.pose.orientation.x = odo_rot[0]
robot_odo.pose.orientation.y = odo_rot[1]
robot_odo.pose.orientation.z = odo_rot[2]
robot_odo.pose.orientation.w = odo_rot[3]
self.pub_odom.publish(robot_odo)
def parseUrdfJoint(xml, jointNames, jonintTree):
'''获取每个关节的实际位姿'''
matrix = OrderedDict()
result = OrderedDict()
qmatrix = OrderedDict()
# axises=[]
#获取关节参数
for jname in jointNames:
jointNode = xml.xpath("//joint[@name='%s']" % jname)[0]
originNode = jointNode.find("origin")
axisNode = jointNode.find("axis")
origin_xyz = [0.0, 0.0, 0.0] if originNode is None else map(
float, originNode.attrib['xyz'].split())
origin_rpy = [0.0, 0.0, 0.0] if originNode is None else map(
float, originNode.attrib['rpy'].split())
# axis_xyz=[0.0,0.0,1.0] if axisNode is None else map(float,axisNode.attrib['xyz'].split())
mat = compose_matrix(angles=origin_rpy, translate=origin_xyz)
pname = getParentJointName(jonintTree, jname) #获取当前关节的父关节名称
if pname is None:
matrix[jname] = mat
else:
matrix[jname] = matrix[pname].dot(mat)
scale, shear, angles, translate, perspective = decompose_matrix(
matrix[jname])
result[jname] = [
np.array(translate).round(8),
np.array(angles).round(8)
]
qmatrix[jname] = [
translate.round(8),
quaternion_from_euler(*angles).round(8)
]
# axises.append(axis_xyz)
return result
def hmat_to_trans_rot(hmat):
'''
Converts a 4x4 homogenous rigid transformation matrix to a translation and a
quaternion rotation.
'''
scale, shear, angles, trans, persp = transformations.decompose_matrix(hmat)
rot = transformations.quaternion_from_euler(*angles)
return trans, rot
def matrix_to_transform(H):
''' Convert a 4x4 homogeneous transform matrix to a ros Transform message '''
scale, shear, angles, trans, persp = transformations.decompose_matrix(H)
q = transformations.quaternion_from_euler(*angles)
transform_msg = Transform()
transform_msg.translation = Vector3(*trans)
transform_msg.rotation = Quaternion(*q)
return transform_msg
def matrix_to_rot_trans(transform):
'''
Converts a 4x4 homogenous rigid transformation matrix to a translation and a
quaternion rotation.
'''
scale, shear, angles, trans, persp = transformations.decompose_matrix(
transform)
rot = transformations.quaternion_from_euler(*angles)
return rot, trans
def T_to_tf(T):
"""Convert a 4x4 transformation matrix to a (tx, ty, tz, rx, ry, rz) tuple."""
if T.shape == (4, 4):
# Single transform
_, _, rot, trans, _ = transformations.decompose_matrix(T)
return np.hstack((trans, rot))
else:
# Array of transforms
assert T.shape[1:] == (4, 4), "Transform array must be Nx4x4"
return np.array([T_to_tf(row) for row in T])
def odom_callback(msg):
# global cur_pos, cur_heading
pose_msg = msg.pose.pose
pose = numpify(pose_msg)
__, __, angles, translate, __ = decompose_matrix(pose)
cur_pos = translate[0:2]
cur_heading = angles[2]
def _invert_pose(pose):
"""Invert a given pose as if it is a transformation.
Args:
pose (geometry.Pose): A pose to be inverted.q
Returns:
geometry.Pose: The result of computation
"""
m = T.compose_matrix(translate=pose[0],
angles=T.euler_from_quaternion(pose[1]))
(_, _, euler, trans, _) = T.decompose_matrix(T.inverse_matrix(m))
q = T.quaternion_from_euler(euler[0], euler[1], euler[2])
return geometry.Pose(trans, q)
def ar_pose_marker_callback(msg):
if len(msg.markers) == 0:
return
pose_msg = msg.markers[0].pose.pose
pose = numpify(pose_msg)
__, __, angles, translate, __ = decompose_matrix(pose)
cur_pos = translate[0:2]
cur_heading = angles[2]
print "ar: " + str(angles)
def match_markers(image_coords, object_coords, camera_pose_prior,
camera_matrix, distortion_coeffs):
"""Return image_coords sorted into the same order as projected_coords."""
_, _, rotation, translation, _ = transformations.decompose_matrix(
np.linalg.inv(camera_pose_prior))
rotation = transformations.euler_matrix(*rotation)
rotation, _ = cv2.Rodrigues(rotation[:3, :3])
projected_coords, _ = cv2.projectPoints(object_coords, rotation,
translation, camera_matrix,
distortion_coeffs)
projected_coords = projected_coords.squeeze()
tree = scipy.spatial.cKDTree(projected_coords)
dists, indices = tree.query(image_coords, k=1)
return object_coords[indices]
def compute(self, links, joint_angles, joint_poses=None, setup=False):
# Check dimensions
assert len(joint_angles) >= 4
# Update joint angles
self.update_joints(joint_angles, joint_poses)
# Links and joint poses
pose = np.eye(4)
link_cuboids = links[:1]
# Compute forward kinematics
for i, joint in enumerate(self.joints):
# Compute joint pose
R, T, axis = self.robot.get_joint_pose(joint.name)
pose = np.matmul(pose, np.matmul(np.matmul(R, T), joint.offset_matrix))
# Update arm joints
if i < 5:
wrist_pose = pose
# Setup link to joint transforms
if setup:
self.link_to_joints.append(np.matmul(np.linalg.inv(wrist_pose), links[i + 1].transform_matrix))
# Compute link pose
else:
link_pose = np.matmul(wrist_pose, self.link_to_joints[i])
_, _, R, T, _ = decompose_matrix(link_pose)
# Create link cuboids
link_cuboids.append(Cuboid(T, R, links[i + 1].dxyz, links[i + 1].name))
# Return wrist pose and link cuboids
_, _, R, T, _ = decompose_matrix(wrist_pose)
return R, T, link_cuboids
def extract2d_and_cleanup(data):
probe_radius = 0.004745 # probe1: 0.00626/2 probe2: 0.004745
tip_pose = data['tip_poses']
object_pose = data['object_pose']
ft = data['ft_wrench']
# transformation to the first
invT0 = np.linalg.inv(matrix_from_xyzquat(object_pose[0][1:4], object_pose[0][4:8]))
sub = 1
# object
object_pose_2d = []
for i in (range(0, len(object_pose), sub)):
T = matrix_from_xyzquat(object_pose[i][1:4], object_pose[i][4:8])
tip_pose_object0 = np.dot(invT0, T)
scale, shear, angles, trans, persp = tfm.decompose_matrix(tip_pose_object0)
#print 'trans', trans[0:2], 'angle', angles[2]
time = object_pose[i][0]
# don't add redundant data entry with the same time
if(not(len(object_pose_2d) > 0 and time == object_pose_2d[-1][0] )):
object_pose_2d.append([time] + trans[0:2].tolist() + [angles[2]])
# probe
tip_pose_2d = []
for i in (range(0, len(tip_pose), sub)):
tip_pose_0 = np.dot(invT0, tip_pose[i][1:4]+[1])
#print 'trans', tip_pose_0[0:2]
time = tip_pose[i][0]
# don't add redundant data entry with the same time
if(not(len(tip_pose_2d) > 0 and time == tip_pose_2d[-1][0] )):
tip_pose_2d.append([time] + tip_pose_0[0:2].tolist())
# ft, no redundency
ft_2d = np.array(ft)[:,0:3].tolist() # only need the force
print 'object_pose_2d', len(object_pose_2d), 'tip_pose_2d', len(tip_pose_2d), 'ft_2d', len(ft_2d)
data2d = {}
data2d['tip_poses_2d'] = tip_pose_2d
data2d['object_poses_2d'] = object_pose_2d
data2d['force_2d'] = ft_2d
return data2d
def match_markers(image_coords, object_coords, camera_pose_prior, camera_matrix, distortion_coeffs):
"""Return image_coords sorted into the same order as projected_coords."""
# assert len(image_coords) == len(object_coords), "Image, object coords not same length. {} != {}".format(
# len(image_coords), len(object_coords))
_, _, rotation, translation, _ = transformations.decompose_matrix(np.linalg.inv(camera_pose_prior))
rotation = transformations.euler_matrix(*rotation)
rotation, _ = cv2.Rodrigues(rotation[:3, :3])
projected_coords, _ = cv2.projectPoints(object_coords, rotation, translation, camera_matrix, distortion_coeffs)
projected_coords = projected_coords.squeeze()
projected_coords
tree = scipy.spatial.cKDTree(projected_coords)
dists, indices = tree.query(image_coords, k=1)
output_image = np.zeros((1080, 1920), dtype="uint8")
output_image = cv2.cvtColor(output_image, cv2.COLOR_GRAY2BGR)
# for proj, img in zip(image_coords, projected_coords[indices]):
# output_image = cv2.circle(output_image, (int(proj[0]), int(proj[1])), 1, (255, 0, 0))
# output_image = cv2.circle(output_image, (int(img[0]), int(img[1])), 1, (0, 255, 0))
# cv2.imshow("a", output_image)
# cv2.waitKey(0)
return object_coords[indices]
def create_tf_transform_from_pose(to_pose, from_pose):
q_from = from_pose.orientation
q_to = to_pose.orientation
T_from = quaternion_matrix( \
np.array([q_from.x, q_from.y, q_from.z, q_from.w], dtype=np.float64))
T_from[0, 3] = from_pose.position.x
T_from[1, 3] = from_pose.position.y
T_from[2, 3] = from_pose.position.z
T_to = quaternion_matrix( \
np.array([q_to.x, q_to.y, q_to.z, q_to.w], dtype=np.float64))
T_to[0, 3] = to_pose.position.x
T_to[1, 3] = to_pose.position.y
T_to[2, 3] = to_pose.position.z
T_from_to_to = np.matmul(T_to, np.linalg.inv(T_from))
_, _, angles, translate, _ = decompose_matrix(T_from_to_to)
q_from_to_to = quaternion_from_euler(angles[0], angles[1], angles[2])
return (translate, q_from_to_to)
def match_checkerboard_corners(image_coords, object_coords, t_mo_prior,
camera_matrix, distortion_coeffs):
"""Utilizes the fact that the image coords and object coords are in the same order bar missing corners"""
# This is the worst case for how wrong the indices are
diff = len(object_coords) - len(image_coords)
_, _, rotation, translation, _ = transformations.decompose_matrix(
np.linalg.inv(t_mo_prior))
rotation = transformations.euler_matrix(*rotation)
rotation, _ = cv2.Rodrigues(rotation[:3, :3])
projected_coords, _ = cv2.projectPoints(object_coords, rotation,
translation, camera_matrix,
distortion_coeffs)
projected_coords = projected_coords.squeeze()
# Need to make sure the checkerboard is the right way round
error_1 = np.linalg.norm(image_coords[0] - projected_coords[0])
error_2 = np.linalg.norm(image_coords[0] - projected_coords[-1])
if error_2 < error_1:
projected_coords = projected_coords[::-1]
object_coords = object_coords[::-1]
return object_coords
def create_transform_from_pose(to_xyzrpy, from_xyzrpy):
q_from = quaternion_from_euler(from_xyzrpy.roll, from_xyzrpy.pitch,
from_xyzrpy.yaw)
q_to = quaternion_from_euler(to_xyzrpy.roll, to_xyzrpy.pitch,
to_xyzrpy.yaw)
T_from = quaternion_matrix( \
np.array([q_from[0], q_from[1], q_from[2], q_from[3]], dtype=np.float64))
T_from[0, 3] = from_xyzrpy.x
T_from[1, 3] = from_xyzrpy.y
T_from[2, 3] = from_xyzrpy.z
T_to = quaternion_matrix( \
np.array([q_to[0], q_to[1], q_to[2], q_to[3]], dtype=np.float64))
T_to[0, 3] = to_xyzrpy.x
T_to[1, 3] = to_xyzrpy.y
T_to[2, 3] = to_xyzrpy.z
T_from_to_to = np.matmul(T_to, np.linalg.inv(T_from))
_, _, angles, translate, _ = decompose_matrix(T_from_to_to)
q_from_to_to = quaternion_from_euler(angles[0], angles[1], angles[2])
return (translate, q_from_to_to)
def odom_callback(self, msg):
tb_pose = msg.pose.pose
__, __, angles, position, __ = decompose_matrix(numpify(tb_pose))
self.pose = [position[0:2], angles[2]]
def odom_callback(self, msg):
tb_pose = msg.pose.pose
self.tb_pose = numpify(tb_pose)
__, __, angles, position, __ = decompose_matrix(numpify(tb_pose))
self.tb_position = position[0:2]
self.tb_rot = angles
def main(argv):
parser = optparse.OptionParser()
parser.add_option('-s', action="store", dest='shape_id',
help='The shape id e.g. rect1-rect3', default='rect1')
parser.add_option('', '--surface', action="store", dest='surface_id',
help='The surface id e.g. plywood, abs', default='plywood')
parser.add_option('', '--nrep', action="store", dest='nrep', type='int',
help='Repeat how many times',
default=2000)
parser.add_option('', '--reptype', action="store", dest='reptype', type='string',
help='Repeat type',
default='normal')
(opt, args) = parser.parse_args()
if len(args) < 1: # no bagfile name
parser.error("Usage: plot_rep_pushes.py [dir_to_rep_push] e.g. /home/mcube/pnpushdata/straight_push_rep")
return
dir_to_rep_push = args[0]
vel = 20
acc = 0
i = 0
s = 0.7
t = 0
figfname_png = dir_to_rep_push + '/rep_push_viz_%s.png' % opt.surface_id
figfname_pdf = dir_to_rep_push + '/rep_push_viz_%s.pdf' % opt.surface_id
figfname_2_png = dir_to_rep_push + '/rep_push_viz_2_%s.png' % opt.surface_id
figfname_2_pdf = dir_to_rep_push + '/rep_push_viz_2_%s.pdf' % opt.surface_id
figfname_hist_png = dir_to_rep_push + '/rep_push_viz_hist_%s.png' % opt.surface_id
figfname_hist_pdf = dir_to_rep_push + '/rep_push_viz_hist_%s.pdf' % opt.surface_id
cachefile = '/tmp/plot_rep_push_%s' % opt.surface_id
import shelve
if os.path.exists(cachefile):
f = shelve.open(cachefile)
vals = f['vals'];
#trajs = f['trajs'];
#fts = f['fts']
opt = f['opt']
#trajs_tippose = f['trajs_tippose']
meantraj = f['meantraj']
meantraj_tippose = f['meantraj_tippose']
else:
vals = [] # delta between start and end
trajs = []
trajs_tippose = []
fts = []
for rep in xrange(opt.nrep):
print rep
h5filename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.h5' % (opt.surface_id, opt.shape_id, acc*1000, vel, i, s, t, rep)
#filename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], i, s, t, rep)
filepath = '%s/%s/%s/%s/%s' % (dir_to_rep_push,opt.surface_id,opt.shape_id,opt.reptype,h5filename)
if not os.path.isfile(filepath):
print 'not exists', filepath
continue
f = h5py.File(filepath, "r")
ft_array = f['ft_wrench'].value
object_pose = f['object_pose'].value
tip_pose = f['tip_pose'].value
f.close()
invT0 = np.linalg.inv(matrix_from_xyzrpy(object_pose[0][1:3].tolist() + [0], [0,0,object_pose[0][3]]))
T = matrix_from_xyzrpy(object_pose[-1][1:3].tolist() + [0], [0,0,object_pose[-1][3]])
T_T0 = np.dot(invT0, T)
scale, shear, angles, trans, persp = tfm.decompose_matrix(T_T0)
vals.append(np.append(trans[0:2] ,np.unwrap([angles[2]])))
# extract traj
#if rep in xrange(500):
if True:
traj = []
for p in object_pose:
T = matrix_from_xyzrpy(p[1:3].tolist() + [0], [0,0,p[3]])
T_T0 = np.dot(invT0, T)
scale, shear, angles, trans, persp = tfm.decompose_matrix(T_T0)
traj.append(np.append([p[0]-object_pose[0][0]], np.append(trans[0:2] , np.unwrap([angles[2]]))) )
trajs.append(traj)
traj_tippose = []
for tip_pose_ in tip_pose:
traj_tippose.append(np.append([tip_pose_[0]-tip_pose[0][0]], np.dot(invT0, tip_pose_[1:3].tolist()+[0,1])))
trajs_tippose.append(traj_tippose)
def computeMeanTraj(trajs):
lenn = 1000000
for traj in trajs:
lenn = min(lenn, len(traj))
ncol = len(trajs[0][0])
meantraj = np.zeros((lenn, ncol))
ntraj = len(trajs)
for traj in trajs:
meantraj = meantraj + np.array(traj[0:lenn]) / ntraj
return meantraj
meantraj = computeMeanTraj(trajs)
meantraj_tippose = computeMeanTraj(trajs_tippose)
ll = locals()
shv = shelve.open(cachefile, 'n')
for key, val in ll.iteritems():
try:
shv[key] = val
except:
pass
shv.close()
(x,y,th)=zip(*(vals))
valscov = np.cov(vals, rowvar=0)
valsmean = np.mean(vals, axis=0)
print 'covariance\n', valscov
print 'mean', valsmean
print 'mean', valsmean[0:2] * 1000, 'mm', np.rad2deg(valsmean[2]), 'deg'
eigvs,eigvec = eigsorted(valscov[0:2][:,0:2])
print 'error_trans:', np.sqrt(eigvs[0] + eigvs[1]) *1000 , 'mm'
print 'error_percent_trans:', np.sqrt(eigvs[0] + eigvs[1]) / np.sqrt(valsmean[0]**2+ valsmean[1]**2) *100 , '%'
print 'error_rot:', np.rad2deg(np.sqrt(valscov[2][2])), 'deg'
print 'error_percent_rot:', np.sqrt(valscov[2][2]) / np.sqrt(valsmean[2]**2) *100 , '%'
#from latexify import latexify; latexify(fig_width=3.39, fig_height=3.39*(sqrt(5)-1.0)/2.0*2,scale = 2)
from latexify import latexify; latexify(scale = 2)
#### add the object as polygon
shape_db = ShapeDB()
shape = shape_db.shape_db[opt.shape_id]['shape'] # shape of the objects presented as polygon.
shape_type = shape_db.shape_db[opt.shape_id]['shape_type']
if shape_type == 'poly':
shape_polygon_3d = np.hstack((np.array(shape), np.zeros((len(shape), 1)), np.ones((len(shape), 1))))
elif shape_type == 'ellip':
shape = shape[0]
elif shape_type == 'polyapprox':
shape_polygon_3d = np.hstack((np.array(shape[0]), np.zeros((len(shape[0]), 1)), np.ones((len(shape[0]), 1))))
part1 = False
if part1:
f1, ((ax1, ax2)) = plt.subplots(1, 2, sharex=True, sharey=True)
#fig = plt.figure()
plt.sca(ax1)
ax = ax1
(tm, x,y,th) = zip(*meantraj)
line = plt.plot(x, y, '-k')
plt.setp(line, linewidth=2)
ec, fc = 'black','orangered'
for ii in np.linspace(0, len(meantraj)-1, 30):
i = int(ii)
if i == 0:
alpha , fill = (0.3, True)
elif i == len(meantraj)-1:
alpha , fill = (0.6, True)
else:
alpha , fill = (0.6, False)
T = matrix_from_xyzrpy([x[i], y[i], 0], [0, 0, th[i]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:,0:2], closed=True, fc=fc, ec=ec, alpha=alpha, fill=fill, linewidth=1, linestyle='solid')
ax.add_patch(obj)
#####
###add the probes as circle
probe_radius = 0.00475
for ind, ii in enumerate(np.linspace(0, len(meantraj_tippose)-1, 30)):
i = int(ii)
if opt.surface_id == 'abs' and ind < 4: # hack
continue
if i == 0:
alpha , fill = (0.8, False)
elif i == len(meantraj_tippose)-1:
alpha , fill = (0.8, False)
else:
alpha , fill = (0.8, False)
circle = mpatches.Circle(meantraj_tippose[i][1:3], probe_radius, color='black', alpha=alpha, fill=fill, linewidth=1, linestyle='solid')
ax.add_patch(circle)
plt.axis('equal')
plt.axis('off')
# ##2. plot all traj
ax = ax2
plt.sca(ax)
for traj in trajs:
(tm, x,y,th) = zip(*traj)
plt.plot(x, y, 'g', alpha=0.5)
# ##plot begin and final mean block
(tm,x,y,th) = zip(*meantraj)
for i in [0, -1]:
alpha , fill = (0.6, False)
T = matrix_from_xyzrpy([x[i], y[i], 0], [0, 0, th[i]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:,0:2], closed=True, fc=fc, ec=ec, alpha=alpha, fill=fill, linewidth=1, linestyle='solid', zorder=2)
ax.add_patch(obj)
line = plt.plot(x, y, '-k')
plt.setp(line, linewidth=2)
plot_cov_ellipse(valscov[0:2][:,0:2], valsmean[0:2], color='orangered', fill=True, alpha=0.9, zorder=3)
#import pdb; pdb.set_trace()
#plot_cov_ellipse(valscov[0:2][:,0:2], meantraj[-1][1:3], color='orangered', fill=True, alpha=0.9, zorder=3)
plt.axis('equal')
plt.axis('off')
plt.savefig(figfname_png, dpi=200)
plt.savefig(figfname_pdf)
## 3. plot final poses
f2, ((ax3, ax4)) = plt.subplots(1, 2, sharex=False, sharey=False)
ax = ax3
plt.sca(ax)
(xd,yd,thd)=zip(*(vals))
ax.scatter(xd,yd, s=0.2, color='k', alpha=1)
### plot begin and final mean block
ec, fc = 'black','orangered'
(tm,x,y,th) = zip(*meantraj)
for i in [0,-1]:
alpha , fill = (0.6, False)
T = matrix_from_xyzrpy([x[i], y[i], 0], [0, 0, th[i]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
#obj = mpatches.Polygon(shape_polygon_3d_world.T[:,0:2], closed=True, fc=fc, ec=ec, alpha=alpha, fill=fill, linewidth=1, linestyle='solid')
#ax.add_patch(obj)
### plot 2 sigma bound
plot_cov_ellipse(valscov[0:2][:,0:2], valsmean[0:2], color='orangered', fill=True, alpha=0.9, zorder=0)
##plot_cov_ellipse(valscov[0:2][:,0:2], valsmean[0:2], 3, color='orangered', fill=True, alpha=0.5, zorder=0)
##ax.add_patch(obj)
ax.set_ylim([0,1000])
plt.axis('equal')
plt.axis('off')
##ax2.set_title('Scatter plot: $\Delta x$ versus $\Delta y$')
plt.tight_layout(pad=0, w_pad=0, h_pad=0)
plt.subplots_adjust(left=0.08, bottom=0.06, right=0.97, top=1.0,
wspace=0.01, hspace=0.20)
ax = ax4
plt.sca(ax)
## plot begin and final mean block
(tm,x,y,th) = zip(*meantraj)
for i in [0,1]:
alpha , fill = (0.6, False)
T = matrix_from_xyzrpy([x[i], y[i], 0], [0, 0, th[i]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:,0:2], closed=True, fc=fc, ec=ec, alpha=alpha, fill=fill, linewidth=1, linestyle='solid', zorder=2)
ax.add_patch(obj)
line = plt.plot(x, y, '-k')
plt.setp(line, linewidth=2)
## plot simulated data
(x_sim,y_sim,th_sim) = zip(*get_sim_data())
line_sim = plt.plot(x_sim, y_sim, '--k')
plt.setp(line_sim, linewidth=2)
T = matrix_from_xyzrpy([x_sim[-1], y_sim[-1], 0], [0, 0, th_sim[-1]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:,0:2], closed=True, fc=fc, ec=ec, alpha=alpha, fill=fill, linewidth=1, linestyle='dashed', zorder=2)
ax.add_patch(obj)
####
#ax.set_ylim([-0.3,0.05])
plt.axis('equal')
plt.axis('off')
plt.tight_layout(pad=0, w_pad=0, h_pad=0)
# ax.set_xlim([-0.2,0.2])
# ax.set_ylim([-0.3,0.05])
plt.savefig(figfname_2_png, dpi=200)
plt.savefig(figfname_2_pdf)
plt.show()
# 5-7 plot histogram
f3, ((ax5, ax6, ax7)) = plt.subplots(1, 3, sharex=False, sharey=False)
plt.sca(ax5)
plt.locator_params(axis='x',nbins=4)
n, bins, patches= ax5.hist(np.array(xd)*1000, 200, normed=1, histtype='stepfilled',
facecolor='none', label='x', alpha=1)
ax5.get_yaxis().set_visible(False)
ax5.set_xlabel('$\Delta x$ (mm)')
#ax5.set_title('Histogram of $\Delta x$')
plt.sca(ax6)
plt.locator_params(axis='x',nbins=4)
n, bins, patches= ax6.hist(np.array(yd)*1000, 200, normed=1, histtype='stepfilled',
facecolor='none', label='y', alpha=1)
ax6.get_yaxis().set_visible(False)
ax6.set_xlabel('$\Delta y$ (mm)')
#ax6.set_title('Histogram of $\Delta y$')
plt.sca(ax7)
plt.locator_params(axis='x',nbins=4)
n, bins, patches= ax7.hist(np.rad2deg(thd), 200, normed=1, histtype='stepfilled',
facecolor='none', label='theta', alpha=1)
ax7.get_yaxis().set_visible(False)
ax7.set_xlabel('$\Delta \\theta$ (degree)')
#ax7.set_title('Histogram of $\Delta \\theta$')
plt.tight_layout(pad=0, w_pad=0, h_pad=0)
plt.subplots_adjust(left=0.04, bottom=0.23, right=0.96, top=0.87,
wspace=0.22, hspace=0.20)
plt.savefig(figfname_hist_png, dpi=200)
plt.savefig(figfname_hist_pdf)
plt.show()
def __call__(self, img, pose,
in_thresh = 16,
s = 0.1
):
# == 0 : unroll parameters ==
self.K_ = self.K_full_ if (img.shape[0] == 480) else self.K_half_
Kmat = self.K_
distCoeffs = self.dC_
method = self.method_ # TODO : configure
# ============================
# detect features + query/update history
img2 = img
kpt2, des2 = self.detect(img2)
if len(self.hist_) <= 0:
self.lm_msk_ = np.zeros(len(kpt2), dtype=np.bool)
self.hist_.append( (kpt2, des2, img2, pose) )
return True, None
kpt1, des1, img1, pose1 = self.hist_[-1]
self.hist_.append((kpt2, des2, img2, pose))
# ==============
msg = ''
reinit_thresh = 100
# TODO : potentially incorporate descriptors match information at some point.
if len(self.lm_pt2_) >= reinit_thresh:
# has sufficient # of landmarks to track
# TODO : configure number of required landmark points
pt1 = self.lm_pt2_ # use most recent tracking points
pt2_0 = self.estimate_initial_flow(
pt1, cv2.KeyPoint_convert(kpt2),
self.lm_des_, des2)
pt2, msk = self.track(img1, img2, pt1, pt2=pt2_0)
track_ratio = float(msk.sum()) / msk.size
print 'track status : {}%'.format(track_ratio * 100)
msg += ('track : %.2f' % (track_ratio * 100))
msg += '% | '
if track_ratio > 0.85:
self.lm_pt3_ = self.lm_pt3_[msk]
self.lm_des_ = self.lm_des_[msk]
self.lm_pt2_ = pt2[msk] # update tracking point
# # update landmark positions
res = self.getPoseAndPoints(
self.undistort(pt2[msk]),
self.undistort(pt1[msk]),
np.arange(msk.sum()), method)
if res is not None:
R, t, _, _, midx, pt3_h = res
# pt3_h w.r.t pose
lm_pt3_c0 = self.lm_pt3_[midx] # landmark w.r.t c0
# to estimate scale, c0 -> c1 required
lm_pt3_c0_h = cv2.convertPointsToHomogeneous(lm_pt3_c0)
lm_pt3_c0_h = np.squeeze(lm_pt3_c0_h, axis=1)
lm_pt3_c1 = np.linalg.multi_dot([
lm_pt3_c0_h,
self.T_c2b_.T, # now base0 coordinates
tx.inverse_matrix(self.T_b2o(pose)).T, # now base1 coordinates
self.T_b2c_.T # now cam1 coordinates
])
prv = cv2.convertPointsFromHomogeneous(lm_pt3_c1)
prv = np.squeeze(prv, axis=1)
cur = cv2.convertPointsFromHomogeneous(pt3_h.T)
cur = np.squeeze(cur, axis=1)
d_prv = np.linalg.norm(prv, axis=-1)
d_cur = np.linalg.norm(cur, axis=-1)
# override input scale information
ss = d_prv / d_cur
#print('scale estimates : {}'.format(ss))
print('input scale : {}'.format(s))
s2 = np.median(ss)
print('computed scale : {}'.format(s2))
msg += 'scale : {:.2f}% | '.format(s2/s * 100)
# update s with computed scale
s = s2
cur *= s
cur_h = cv2.convertPointsToHomogeneous(cur)
cur_h = np.squeeze(cur_h, axis=1)
lm_pt3 = np.linalg.multi_dot([
cur_h,
self.T_c2b_.T, # now base1 coordinates
self.T_b2o(pose).T, # now base0 coordinates
self.T_b2c_.T # now cam0 coordinates
])[:,:3]
print 'update landmark position'
alpha = 0.9
self.lm_pt3_[midx] = (
alpha * self.lm_pt3_[midx] + (1.0-alpha) * lm_pt3)
# == dynamically add points ==
# TODO : currently super unstable
# augment landmark points while we're at it
if True:#False: # make this "True" to try dynamically adding points
match_f2m = self.match(des1, self.lm_des_,
lowe_ratio=1.0,
thresh = 128.0
)
i1_f2m, i2_f2m = np.int32([
(m.queryIdx, m.trainIdx)
for m in match_f2m]).T
# select points from frame 1
# that are not already in landmarks
msk1 = np.ones(len(des1), dtype=np.bool)
msk1[i1_f2m] = False
print 'points to use : {}'.format(msk1.sum())
# not near any of currently tracking points either
neigh = NearestNeighbors(n_neighbors=1)
neigh.fit(self.lm_pt2_)
d, i = neigh.kneighbors(cv2.KeyPoint.convert(kpt1), return_distance=True)
msk1[(d < 20.0)[:,0]] = False
if msk1.sum() > 10:
#p1_n = cv2.KeyPoint.convert(kpt1[msk1])
#p2_n, msk_f2f = self.track(img1, img2, p1_n)
match_f2f = self.match(des1[msk1], des2)
i1_f2f, i2_f2f = np.int32([
(m.queryIdx, m.trainIdx)
for m in match_f2f]).T
p1_n = cv2.KeyPoint.convert(kpt1[msk1][i1_f2f])
p2_n, msk_f2f = self.track(img1, img2, p1_n,
pt2 = cv2.KeyPoint.convert(kpt2[i2_f2f])
)
#p2_n = cv2.KeyPoint.convert(kpt2[i2_f2f])
#msk_f2f = np.ones(len(p1_n), dtype=np.bool)
try:
res_f2f = self.getPoseAndPoints(
self.undistort(p1_n[msk_f2f]),
self.undistort(p2_n[msk_f2f]),
np.arange(msk_f2f.sum()), method)
if res_f2f is not None:
midx_f2f, pt3_h_f2f = res_f2f[-2:]
pts_n = cv2.convertPointsFromHomogeneous(pt3_h_f2f.T)
pts_n = np.squeeze(pts_n, axis=1)
pts_n *= s
pts_n_h = cv2.convertPointsToHomogeneous(pts_n)
pts_n_h = np.squeeze(pts_n_h, axis=1)
lm_pt3_n = np.linalg.multi_dot([
pts_n_h,
self.T_c2b_.T, # now base1 coordinates
self.T_b2o(pose1).T, # now base0 coordinates
self.T_b2c_.T # now cam0 coordinates
])[:,:3]
print 'adding {} points'.format(len(lm_pt3_n))
msg += ('+p=%d ' % len(lm_pt3_n))
self.lm_pt2_ = np.concatenate(
[self.lm_pt2_, p1_n[msk_f2f][midx_f2f]], axis=0)
self.lm_pt3_ = np.concatenate(
[self.lm_pt3_, lm_pt3_n], axis=0)
self.lm_des_ = np.concatenate(
[self.lm_des_, des1[msk1][i1_f2f][msk_f2f][midx_f2f]])
except Exception as e:
print 'Adding Landmarks Failed : {}'.format(e)
else:
# force landmarks re-initialization
print('Force Reinitialization')
self.tracking_ = False
if (self.tracking_ is False) or len(self.lm_pt2_) < reinit_thresh:
self.tracking_ = True # TODO : figure out if tracking is really true
print('re-initializing landmarks')
# requires 3D landmarks initialization
# extract points from keypoints
pt1 = cv2.KeyPoint.convert(kpt1)
pt2_0 = self.estimate_initial_flow(
pt1, cv2.KeyPoint.convert(kpt2),
des1, des2)
#pt2 = cv2.KeyPoint.convert(kpt2)
# requires track landmarks initialization
pt2, msk = self.track(img1, img2, pt1)
track_ratio = float(msk.sum()) / msk.size
print 'track status : {}%'.format(track_ratio * 100)
if track_ratio < 0.8:
# track failed - attempt match
print('attempting match ...')
matches = self.match(des1, des2)
i1, i2 = np.int32([(m.queryIdx, m.trainIdx) for m in matches]).T
# grab relevant keypoints + points
pt1 = np.float32(cv2.KeyPoint.convert(kpt1[i1]))
pt2 = np.float32(cv2.KeyPoint.convert(kpt2[i2]))
print('match result : {}/{}'.format(len(matches), len(kpt1)))
des1 = des1[i1]
des2 = des2[i2]
msk = np.ones(len(pt1), dtype=np.bool)
# apply mask prior to triangulation
pt1_l, pt2_l = pt1[msk], pt2[msk]
pt1_u, pt2_u = self.undistort(pt1_l), self.undistort(pt2_l)
midx = np.arange(len(pt1_l))
# landmarks triangulation
res = self.getPoseAndPoints(pt1_u, pt2_u, midx, method)
R, t, _, _, midx, pt3_h = res
lm_pt4 = (pt3_h / pt3_h[3:]).T
if self.lm_des_ is not None:
# estimate scale from prior landmark matches
match_lm = self.match(self.lm_des_, des1[msk][midx])
print('prv landmarks match : {}'.format( len(match_lm) ))
if len(match_lm) >= 1: # TODO : adjust this parameter
i1, i2 = np.int32([(m.queryIdx, m.trainIdx) for m in match_lm]).T
# sufficient landmark matches are required to apply scale
lm_pt3_c0 = self.lm_pt3_[i1] # landmark w.r.t c0
# to estimate scale, c0 -> c1 required
lm_pt3_c0_h = cv2.convertPointsToHomogeneous(lm_pt3_c0)
lm_pt3_c0_h = np.squeeze(lm_pt3_c0_h, axis=1)
lm_pt3_c1 = np.linalg.multi_dot([
lm_pt3_c0_h,
self.T_c2b_.T, # now base0 coordinates
tx.inverse_matrix(self.T_b2o(pose1)).T, # now base1 coordinates
self.T_b2c_.T # now cam1 coordinates
])
prv = lm_pt3_c1[:,:3] # previous landmark locations
cur = lm_pt4[i2,:3] # persistent identified landmark locations
d_prv = np.linalg.norm(prv, axis=-1)
d_cur = np.linalg.norm(cur, axis=-1)
# override input scale information
ss = d_prv / d_cur
#print('scale estimates : {}'.format(ss))
print('input scale : {}'.format(s))
s = np.median(ss)
print('computed scale : {}'.format(s))
lm_pt4[:,:3] *= s
else:
print 'using input scale due to failure : {}'.format(s)
lm_pt4[:,:3] *= s
else:
# apply input scale (probably only called on initialization)
print 'input scale : {}'.format(s)
#s = 0.02
lm_pt4[:,:3] *= s # HERE is where scale is applied.
# lm_pt3 currently w.r.t cam1 coordinates
lm_pt3 = np.linalg.multi_dot([
lm_pt4,
self.T_c2b_.T, # now base1 coordinates
self.T_b2o(pose1).T, # now base0 coordinates
self.T_b2c_.T # now cam0 coordinates
])[:,:3]
self.lm_pt3_ = lm_pt3
self.lm_pt2_ = pt2_l[midx] # important: use undistorted version (for tracking)
# most recent points observation
self.lm_des_ = des1[msk][midx]
# construct extrinsic guess
T_b2b0_est = tx.compose_matrix(
translate=[ pose[0], pose[1], 0 ],
angles=[0, 0, pose[2]]
)
T_c0c2_est = np.linalg.multi_dot([
self.T_b2c_,
tx.inverse_matrix(T_b2b0_est), # T_b0b2
self.T_c2b_])
rvec0 = cv2.Rodrigues(T_c0c2_est[:3,:3])[0]
tvec0 = T_c0c2_est[:3, 3:]
# compute cam2 pose
res = cv2.solvePnPRansac(
self.lm_pt3_, self.lm_pt2_, self.K_, self.dC_,
#useExtrinsicGuess=False,
useExtrinsicGuess=True,
rvec = rvec0,
tvec = tvec0,
iterationsCount=1000,
reprojectionError=10.0, # TODO : tune these params
confidence=0.9999,
#flags = cv2.SOLVEPNP_EPNP
#flags = cv2.SOLVEPNP_DLS # << WORKS PRETTY WELL (SLOW?)
#flags = cv2.SOLVEPNP_AP3P
flags = cv2.SOLVEPNP_ITERATIVE # << default
#flags = cv2.SOLVEPNP_P3P
#flags = cv2.SOLVEPNP_UPNP
)
dbg, rvec, tvec, inliers = res
print 'PnP inliers : {}/{}'.format( len(inliers), len(self.lm_pt2_) )
msg += 'pnp {}/{} '.format( len(inliers), len(self.lm_pt2_))
# apply PnP inliers and see how well that goes
inliers = inliers[:,0]
self.lm_pt3_ = self.lm_pt3_[inliers]
self.lm_pt2_ = self.lm_pt2_[inliers]
self.lm_des_ = self.lm_des_[inliers]
if len(inliers) < 50:
self.tracking_ = False
# TODO : filter by inliers
# T_c0c2 transforms points in camera origin coordinates
# to camera2 origin coordinates
T_c0c2 = np.eye(4, dtype=np.float32)
T_c0c2[:3,:3] = cv2.Rodrigues(rvec)[0]
T_c0c2[:3,3:] = tvec
T_c2c0 = tx.inverse_matrix(T_c0c2)
T_b2b0 = np.linalg.multi_dot([
self.T_c2b_,
T_c2c0,
self.T_b2c_]) # base2 in base0 coordinate system
_,_,ang,lin,_ = tx.decompose_matrix(T_b2b0)
#ang = tx.euler_from_matrix(T_b2b0)
#lin = tx.translation_from_matrix(T_b2b0)
base_h1 = ang[-1] # w.r.t z-ax
base_t1 = [lin[0], lin[1]] # x-y components
#print base_h1, base_t1
# from here, visualization + validation
# compute reprojection
pts2_rec, _ = cv2.projectPoints(
np.float32(self.lm_pt3_),
np.float32(rvec), # or does it require inverse rvec/tvec?
np.float32(tvec),
cameraMatrix=Kmat,
distCoeffs=distCoeffs,
)
pts2_rec = np.squeeze(pts2_rec, axis=1)
# TODO : compute relative scale factor
#if len(pts3) < in_thresh:
# # insufficient number of inliers to recover pose
# return True, None
# no-slip
# TODO : why is t[0,0] so bad???
# TODO : relax the no-slip constraint by being better at triangulating or something
mim = drawMatches(img1, img2, pt1, pt2, msk)
#mim = np.concatenate([img1,img2], axis=1)
#cv2.drawKeypoints(mim, kpt1[i1][matchesMask], mim, color=(0,0,255))
#cv2.drawKeypoints(mim[:,320:], kpt2[i2][matchesMask], mim[:,320:], color=(0,0,255))
pts3 = self.lm_pt3_.dot(self.T_c2b_[:3,:3].T) + self.T_c2b_[:3,3]
#matchesMask = np.zeros(len(matches), dtype=np.bool)
#matchesMask[midx] = 1
#draw_params = dict(
# matchColor = (0,255,0),
# singlePointColor = (255,0,0),
# flags = 0,
# matchesMask=matchesMask.ravel().tolist()
# )
#mim = cv2.drawMatches(
# img1,kpt1,img2,kpt2,
# matches,None,**draw_params)
#mim = cv2.addWeighted(np.concatenate([img1,img2],axis=1), 0.5, mim, 0.5, 0.0)
#cv2.drawKeypoints(mim, kpt1[i1][matchesMask], mim, color=(0,0,255))
#cv2.drawKeypoints(mim[:,320:], kpt2[i2][matchesMask], mim[:,320:], color=(0,0,255))
print('---')
return True, (mim, base_h1, base_t1, pts2_rec, pts3, msg)
def callback(self, msg_image):
try:
cv_image = self.bridge.imgmsg_to_cv2(msg_image, "bgr8")
except CvBridgeError as e:
print(e)
corners, ids, ripoints = cv2.aruco.detectMarkers(cv_image,self.aruco_dic)
aruco_flag = False
if ids is not None:
aruco_flag = True
rvecs = np.zeros(shape=(len(ids),3))
tvecs = np.zeros(shape=(len(ids),3))
trans_vector = np.zeros(shape=(len(ids),3))
rot_vector = np.zeros(shape=(len(ids),3))
for i in range(len(ids)):
rvec, tvec, _objPoints = cv2.aruco.estimatePoseSingleMarkers(corners[i], self.markerLength, self.camera_matrix, self.dist_coeffs)
t_vec = np.array([tvec[0][0][0],tvec[0][0][1],tvec[0][0][2]])
#rospy.loginfo("t_vec :=\n %s", t_vec)
r_vec = np.array([rvec[0][0][0],rvec[0][0][1],rvec[0][0][2]])
#r_vec = np.array([0,0,0])
#rospy.loginfo("r_vec :=\n %s", r_vec)
cv2.aruco.drawDetectedMarkers(cv_image,corners, ids)
cv_image = cv2.aruco.drawAxis(cv_image, self.camera_matrix, self.dist_coeffs, rvec, tvec, 0.2)
br = tf2_ros.TransformBroadcaster()
t = TransformStamped()
t.header.frame_id = "camera1_link_optical"
t.header.stamp = rospy.Time.now()
t.child_frame_id = "Marker_"+str(int(ids[i]))
t.transform.translation.x = t_vec[0]
t.transform.translation.y = t_vec[1]
t.transform.translation.z = t_vec[2]
q = quaternion_from_euler(r_vec[0], r_vec[1], r_vec[2])
t.transform.rotation.x = q[0]
t.transform.rotation.y = q[1]
t.transform.rotation.z = q[2]
t.transform.rotation.w = q[3]
br.sendTransform(t)
rvecs[i,:] = rvec
tvecs[i,:] = tvec
## Get global position about the origin
(trans, rot) = self.get_aruco_pose(ids[i])
RO_A = compose_matrix(None, None, rot, trans, None) # Aruco Pose
#rospy.loginfo("RO_A :=\n %s", RO_A)
RC_A = compose_matrix(None, None, r_vec, t_vec, None) # Pose from Aruco to Camera Optical link
RA_C = np.linalg.inv(RC_A)
#rospy.loginfo("RC_A :=\n %s", RC_A)
RC_B = self.get_camera_pose_robot() # Camera pose from Robot base link
#rospy.loginfo("RC_B :=\n %s", RC_B)
RA_B = np.matmul(RA_C,RC_B)
#rospy.loginfo("RA_B :=\n %s", RA_B)
RO_B = np.matmul(RO_A,RA_B)
#rospy.loginfo("RO_B :=\n %s", RO_B)
(_,_,rot,trans,_) = decompose_matrix(RO_B)
trans_vector[i,:] = trans
rot_vector[i,:] = rot
#trans_pose = np.array([np.mean(trans_vector[:,0]),np.mean(trans_vector[:,1]),np.mean(trans_vector[:,2])])
#rot_pose = np.array([np.mean(rot_vector[:,0]),np.mean(rot_vector[:,1]),np.mean(rot_vector[:,2])])
trans_pose = np.array([np.mean(self.remove_outliers(trans_vector[:,0])),np.mean(self.remove_outliers(trans_vector[:,1])),np.mean(self.remove_outliers(trans_vector[:,2]))])
rot_pose = np.array([np.mean(self.remove_outliers(rot_vector[:,0])),np.mean(self.remove_outliers(rot_vector[:,1])),np.mean(self.remove_outliers(rot_vector[:,2]))])
self.pose_callback(trans_pose,rot,aruco_flag)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
except CvBridgeError as e:
print(e)
def main(argv):
parser = optparse.OptionParser()
parser.add_option('-s',
action="store",
dest='shape_id',
help='The shape id e.g. rect1-rect3',
default='rect1')
parser.add_option('',
'--surface',
action="store",
dest='surface_id',
help='The surface id e.g. plywood, abs',
default='plywood')
parser.add_option('',
'--nrep',
action="store",
dest='nrep',
type='int',
help='Repeat how many times',
default=2000)
parser.add_option('',
'--reptype',
action="store",
dest='reptype',
type='string',
help='Repeat type',
default='normal')
(opt, args) = parser.parse_args()
if len(args) < 1: # no bagfile name
parser.error(
"Usage: plot_rep_pushes.py [dir_to_rep_push] e.g. /home/mcube/pnpushdata/straight_push_rep"
)
return
dir_to_rep_push = args[0]
vel = 100 #100
acc = 0
i = 0
s = 0.5 #0.1#0.1#0.5 #0.7
t = -0.175 #-0.349#-0.349 #-0.698# #0
figfname_png = dir_to_rep_push + '/rep_push_viz_%s.png' % opt.surface_id
figfname_pdf = dir_to_rep_push + '/rep_push_viz_%s.pdf' % opt.surface_id
figfname_2_png = dir_to_rep_push + '/rep_push_viz_2_%s.png' % opt.surface_id
figfname_2_pdf = dir_to_rep_push + '/rep_push_viz_2_%s.pdf' % opt.surface_id
figfname_hist_png = dir_to_rep_push + '/rep_push_viz_hist_%s.png' % opt.surface_id
figfname_hist_pdf = dir_to_rep_push + '/rep_push_viz_hist_%s.pdf' % opt.surface_id
cachefile = '/tmp/plot_rep_push_%s' % opt.surface_id
import shelve
if os.path.exists(cachefile):
f = shelve.open(cachefile)
vals = f['vals']
#trajs = f['trajs'];
#fts = f['fts']
opt = f['opt']
#trajs_tippose = f['trajs_tippose']
meantraj = f['meantraj']
meantraj_tippose = f['meantraj_tippose']
else:
vals = [] # delta between start and end
trajs = []
trajs_tippose = []
fts = []
for rep in xrange(opt.nrep):
print rep
h5filename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.h5' % (
opt.surface_id, opt.shape_id, acc * 1000, vel, i, s, t, rep)
#filename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], i, s, t, rep)
filepath = '%s/%s/%s/%s/%s' % (dir_to_rep_push, opt.surface_id,
opt.shape_id, opt.reptype,
h5filename)
if not os.path.isfile(filepath):
print 'not exists', filepath
continue
f = h5py.File(filepath, "r")
ft_array = f['ft_wrench'].value
object_pose = f['object_pose'].value
tip_pose = f['tip_pose'].value
f.close()
invT0 = np.linalg.inv(
matrix_from_xyzrpy(object_pose[0][1:3].tolist() + [0],
[0, 0, object_pose[0][3]]))
T = matrix_from_xyzrpy(object_pose[-1][1:3].tolist() + [0],
[0, 0, object_pose[-1][3]])
T_T0 = np.dot(invT0, T)
scale, shear, angles, trans, persp = tfm.decompose_matrix(T_T0)
vals.append(np.append(trans[0:2], np.unwrap([angles[2]])))
# extract traj
#if rep in xrange(500):
if True:
traj = []
for p in object_pose:
T = matrix_from_xyzrpy(p[1:3].tolist() + [0], [0, 0, p[3]])
T_T0 = np.dot(invT0, T)
scale, shear, angles, trans, persp = tfm.decompose_matrix(
T_T0)
traj.append(
np.append([p[0] - object_pose[0][0]],
np.append(trans[0:2],
np.unwrap([angles[2]]))))
trajs.append(traj)
traj_tippose = []
for tip_pose_ in tip_pose:
traj_tippose.append(
np.append([tip_pose_[0] - tip_pose[0][0]],
np.dot(invT0,
tip_pose_[1:3].tolist() + [0, 1])))
trajs_tippose.append(traj_tippose)
def computeMeanTraj(trajs):
lenn = 1000000
for traj in trajs:
lenn = min(lenn, len(traj))
print len(traj)
ncol = len(trajs[0][0])
meantraj = np.zeros((lenn, ncol))
ntraj = len(trajs)
for traj in trajs:
meantraj = meantraj + np.array(traj[0:lenn]) / ntraj
return meantraj
meantraj = computeMeanTraj(trajs)
meantraj_tippose = computeMeanTraj(trajs_tippose)
ll = locals()
'''
shv = shelve.open(cachefile, 'n')
for key, val in ll.iteritems():
try:
shv[key] = val
except:
pass
shv.close()
'''
(x, y, th) = zip(*(vals))
valscov = np.cov(vals, rowvar=0)
valsmean = np.mean(vals, axis=0)
print 'covariance\n', valscov
print 'mean', valsmean
print 'mean', valsmean[0:2] * 1000, 'mm', np.rad2deg(valsmean[2]), 'deg'
eigvs, eigvec = eigsorted(valscov[0:2][:, 0:2])
print 'error_trans:', np.sqrt(eigvs[0] + eigvs[1]) * 1000, 'mm'
print 'error_percent_trans:', np.sqrt(eigvs[0] + eigvs[1]) / np.sqrt(
valsmean[0]**2 + valsmean[1]**2) * 100, '%'
print 'error_rot:', np.rad2deg(np.sqrt(valscov[2][2])), 'deg'
print 'error_percent_rot:', np.sqrt(valscov[2][2]) / np.sqrt(valsmean[2]**
2) * 100, '%'
#from latexify import latexify; latexify(fig_width=3.39, fig_height=3.39*(sqrt(5)-1.0)/2.0*2,scale = 2)
#from latexify import latexify; latexify(scale = 2)
#### add the object as polygon
shape_db = ShapeDB()
shape = shape_db.shape_db[opt.shape_id][
'shape'] # shape of the objects presented as polygon.
shape_type = shape_db.shape_db[opt.shape_id]['shape_type']
if shape_type == 'poly':
shape_polygon_3d = np.hstack((np.array(shape), np.zeros(
(len(shape), 1)), np.ones((len(shape), 1))))
elif shape_type == 'ellip':
shape = shape[0]
elif shape_type == 'polyapprox':
shape_polygon_3d = np.hstack(
(np.array(shape[0]), np.zeros(
(len(shape[0]), 1)), np.ones((len(shape[0]), 1))))
part1 = True
if part1:
f1, ((ax1, ax2)) = plt.subplots(1, 2, sharex=True, sharey=True)
#fig = plt.figure()
plt.sca(ax1)
ax = ax1
(tm, x, y, th) = zip(*meantraj)
line = plt.plot(x, y, '-k')
plt.setp(line, linewidth=2)
ec, fc = 'black', 'orangered'
for ii in np.linspace(0, len(meantraj) - 1, 30):
i = int(ii)
if i == 0:
alpha, fill = (0.3, True)
elif i == len(meantraj) - 1:
alpha, fill = (0.6, True)
else:
alpha, fill = (0.6, False)
T = matrix_from_xyzrpy([x[i], y[i], 0], [0, 0, th[i]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:, 0:2],
closed=True,
fc=fc,
ec=ec,
alpha=alpha,
fill=fill,
linewidth=1,
linestyle='solid')
ax.add_patch(obj)
#####
###add the probes as circle
probe_radius = 0.00475
for ind, ii in enumerate(np.linspace(0,
len(meantraj_tippose) - 1, 30)):
i = int(ii)
if opt.surface_id == 'abs' and ind < 4: # hack
continue
if i == 0:
alpha, fill = (0.8, False)
elif i == len(meantraj_tippose) - 1:
alpha, fill = (0.8, False)
else:
alpha, fill = (0.8, False)
circle = mpatches.Circle(meantraj_tippose[i][1:3],
probe_radius,
color='black',
alpha=alpha,
fill=fill,
linewidth=1,
linestyle='solid')
ax.add_patch(circle)
plt.axis('equal')
plt.axis('off')
# ##2. plot all traj
ax = ax2
plt.sca(ax)
for traj in trajs:
(tm, x, y, th) = zip(*traj)
plt.plot(x, y, 'g', alpha=0.5)
# ##plot begin and final mean block
(tm, x, y, th) = zip(*meantraj)
for i in [0, -1]:
alpha, fill = (0.6, False)
T = matrix_from_xyzrpy([x[i], y[i], 0], [0, 0, th[i]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:, 0:2],
closed=True,
fc=fc,
ec=ec,
alpha=alpha,
fill=fill,
linewidth=1,
linestyle='solid',
zorder=2)
ax.add_patch(obj)
line = plt.plot(x, y, '-k')
plt.setp(line, linewidth=2)
plot_cov_ellipse(valscov[0:2][:, 0:2],
valsmean[0:2],
color='orangered',
fill=True,
alpha=0.9,
zorder=3)
#import pdb; pdb.set_trace()
#plot_cov_ellipse(valscov[0:2][:,0:2], meantraj[-1][1:3], color='orangered', fill=True, alpha=0.9, zorder=3)
plt.axis('equal')
plt.axis('off')
plt.savefig(figfname_png, dpi=200)
plt.savefig(figfname_pdf)
## 3. plot final poses
f2, ((ax3, ax4)) = plt.subplots(1, 2, sharex=False, sharey=False)
ax = ax3
plt.sca(ax)
(xd, yd, thd) = zip(*(vals))
ax.scatter(xd, yd, s=0.2, color='k', alpha=1)
### plot begin and final mean block
ec, fc = 'black', 'orangered'
(tm, x, y, th) = zip(*meantraj)
for i in [0, -1]:
alpha, fill = (0.6, False)
T = matrix_from_xyzrpy([x[i], y[i], 0], [0, 0, th[i]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
#obj = mpatches.Polygon(shape_polygon_3d_world.T[:,0:2], closed=True, fc=fc, ec=ec, alpha=alpha, fill=fill, linewidth=1, linestyle='solid')
#ax.add_patch(obj)
### plot 2 sigma bound
plot_cov_ellipse(valscov[0:2][:, 0:2],
valsmean[0:2],
color='orangered',
fill=True,
alpha=0.9,
zorder=0)
##plot_cov_ellipse(valscov[0:2][:,0:2], valsmean[0:2], 3, color='orangered', fill=True, alpha=0.5, zorder=0)
##ax.add_patch(obj)
ax.set_ylim([0, 1000])
plt.axis('equal')
plt.axis('off')
##ax2.set_title('Scatter plot: $\Delta x$ versus $\Delta y$')
plt.tight_layout(pad=0, w_pad=0, h_pad=0)
plt.subplots_adjust(left=0.08,
bottom=0.06,
right=0.97,
top=1.0,
wspace=0.01,
hspace=0.20)
ax = ax4
plt.sca(ax)
## plot begin and final mean block
(tm, x, y, th) = zip(*meantraj)
for i in [0, 1]:
alpha, fill = (0.6, False)
T = matrix_from_xyzrpy([x[i], y[i], 0], [0, 0, th[i]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:, 0:2],
closed=True,
fc=fc,
ec=ec,
alpha=alpha,
fill=fill,
linewidth=1,
linestyle='solid',
zorder=2)
ax.add_patch(obj)
line = plt.plot(x, y, '-k')
plt.setp(line, linewidth=2)
## plot simulated data
(x_sim, y_sim, th_sim) = zip(*get_sim_data())
line_sim = plt.plot(x_sim, y_sim, '--k')
plt.setp(line_sim, linewidth=2)
T = matrix_from_xyzrpy([x_sim[-1], y_sim[-1], 0], [0, 0, th_sim[-1]])
shape_polygon_3d_world = np.dot(T, shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:, 0:2],
closed=True,
fc=fc,
ec=ec,
alpha=alpha,
fill=fill,
linewidth=1,
linestyle='dashed',
zorder=2)
ax.add_patch(obj)
####
#ax.set_ylim([-0.3,0.05])
plt.axis('equal')
plt.axis('off')
plt.tight_layout(pad=0, w_pad=0, h_pad=0)
# ax.set_xlim([-0.2,0.2])
# ax.set_ylim([-0.3,0.05])
plt.savefig(figfname_2_png, dpi=200)
plt.savefig(figfname_2_pdf)
plt.show()
# 5-7 plot histogram
f3, ((ax5, ax6, ax7)) = plt.subplots(1, 3, sharex=False, sharey=False)
plt.sca(ax5)
plt.locator_params(axis='x', nbins=4)
n, bins, patches = ax5.hist(np.array(xd) * 1000,
200,
normed=1,
histtype='stepfilled',
facecolor='none',
label='x',
alpha=1)
ax5.get_yaxis().set_visible(False)
ax5.set_xlabel('$\Delta x$ (mm)')
#ax5.set_title('Histogram of $\Delta x$')
plt.sca(ax6)
plt.locator_params(axis='x', nbins=4)
n, bins, patches = ax6.hist(np.array(yd) * 1000,
200,
normed=1,
histtype='stepfilled',
facecolor='none',
label='y',
alpha=1)
ax6.get_yaxis().set_visible(False)
ax6.set_xlabel('$\Delta y$ (mm)')
#ax6.set_title('Histogram of $\Delta y$')
plt.sca(ax7)
plt.locator_params(axis='x', nbins=4)
n, bins, patches = ax7.hist(np.rad2deg(thd),
200,
normed=1,
histtype='stepfilled',
facecolor='none',
label='theta',
alpha=1)
ax7.get_yaxis().set_visible(False)
ax7.set_xlabel('$\Delta \\theta$ (degree)')
#ax7.set_title('Histogram of $\Delta \\theta$')
plt.tight_layout(pad=0, w_pad=0, h_pad=0)
plt.subplots_adjust(left=0.04,
bottom=0.23,
right=0.96,
top=0.87,
wspace=0.22,
hspace=0.20)
plt.savefig(figfname_hist_png, dpi=200)
plt.savefig(figfname_hist_pdf)
plt.show()
def from_T(T: np.ndarray):
_, _, angles, trans, _ = tr.decompose_matrix(T)
return TF(t=trans, q=tr.quaternion_from_euler(*angles))
# Project
from transformAngles import transform
if __name__ == "__main__":
args = sys.argv[1:]
if len(args) == 6:
x = float(args[0])
y = float(args[1])
z = float(args[2])
r = float(args[3]) * math.pi / 180.0
p = float(args[4]) * math.pi / 180.0
yw = float(args[5]) * math.pi / 180.0
M = compose_matrix(angles=[r, p, yw], translate=[x, y, z])
M = inverse_matrix(M)
_, _, angles, trans, _ = decompose_matrix(M)
q = transform(angles[0], angles[1], angles[2])
print trans[0], trans[1], trans[2], q[0], q[1], q[2], q[3]
print trans[0], trans[1], trans[
2], angles[0] * 180.0 / math.pi, angles[
1] * 180.0 / math.pi, angles[2] * 180.0 / math.pi
else:
print "Script to compute inverse transform from one frame to another "
print "Usage : transform_frames.py x y z rollDeg pitchDeg yawDeg "
print " -> x' y' z' q0' q1' q2' q3'"
print " -> x' y' z' rollDeg' pitchDeg' yawDeg'"
qmatrix=[]
axises=[]
#获取关节参数
for jname in roboJointNames:
jointNode=xml.xpath("//joint[@name='%s']"%jname)[0]
originNode=jointNode.find("origin")
axisNode=jointNode.find("axis")
origin_xyz=[0.0,0.0,0.0] if originNode is None else map(float,originNode.attrib['xyz'].split())
origin_rpy=[0.0,0.0,0.0] if originNode is None else map(float,originNode.attrib['rpy'].split())
axis_xyz=[0.0,0.0,1.0] if axisNode is None else map(float,axisNode.attrib['xyz'].split())
mat=compose_matrix(angles=origin_rpy,translate=origin_xyz)
if len(matrix):
matrix.append(matrix[-1].dot(mat))
else:
matrix.append(mat)
scale,shear,angles,translate,perspective=decompose_matrix(matrix[-1])
qmatrix.append([translate.round(8),quaternion_from_euler(*angles).round(8)])
axises.append(axis_xyz)
num=0
robot_data=open(robot_out,'rt').readlines()
for lname in roboLinkNames:
data=map(float,robot_data[num].split())
collisionNode=xml.xpath("//link[@name='%s']/collision"%lname)[0]
###############originNode
originNode=collisionNode.find("origin") #添加originNode
def callback(self, msg):
ee_pose = np.array(msg.O_T_EE).reshape((4, 4), order='F')
scale, shear, euler, pose, _ = decompose_matrix(ee_pose)
self.state = np.concatenate([pose, euler, msg.q[:7], msg.dq[:7]])
def odom_callback(msg):
pose_msg = msg.pose.pose
pose = numpify(pose_msg)
__, __, angles, translate, __ = decompose_matrix(pose)
def matrix_to_params(self, H):
# uses euler angles.... assumes we aren't near singularities
scale, sheer, angles, translate, perspective = transformations.decompose_matrix(H)
return np.hstack((translate, angles))
def main(argv):
parser = optparse.OptionParser()
parser.add_option('-s', action="store", dest='shape_id',
help='The shape id e.g. rect1-rect3', default='rect1')
parser.add_option('', '--surface', action="store", dest='surface_id',
help='The surface id e.g. plywood, abs', default='plywood')
parser.add_option('', '--nrep', action="store", dest='nrep', type='int',
help='Repeat how many times',
default=5000)
parser.add_option('', '--reptype', action="store", dest='reptype', type='string',
help='Repeat type',
default='normal')
(opt, args) = parser.parse_args()
if len(args) < 1: # no bagfile name
parser.error("Usage: plot_rep_pushes.py [dir_to_rep_push] e.g. /home/mcube/pnpushdata/straight_push_rep")
return
dir_to_rep_push = args[0]
vel = 20
acc = 0
i = 0
s = 0.7
t = 0
figfname_png = dir_to_rep_push + '/rep_push_%s.png' % opt.surface_id
figfname_pdf = dir_to_rep_push + '/rep_push_%s.pdf' % opt.surface_id
vals = []
trajs = []
fts = []
for rep in xrange(opt.nrep):
h5filename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.h5' % (opt.surface_id, opt.shape_id, acc*1000, vel, i, s, t, rep)
#filename = 'motion_surface=%s_shape=%s_a=%.0f_v=%.0f_i=%.3f_s=%.3f_t=%.3f_rep=%04d.bag' % (opt.surface_id, opt.shape_id, acc*1000, speeds[cnt_acc], i, s, t, rep)
filepath = '%s/%s/%s/%s/%s' % (dir_to_rep_push,opt.surface_id,opt.shape_id,opt.reptype,h5filename)
if not os.path.isfile(filepath):
print 'not exists', filepath
break
f = h5py.File(filepath, "r")
ft_array = f['ft_wrench'].value
object_pose = f['object_pose'].value
f.close()
invT0 = np.linalg.inv(matrix_from_xyzrpy(object_pose[0][1:3].tolist() + [0], [0,0,object_pose[0][3]]))
T = matrix_from_xyzrpy(object_pose[-1][1:3].tolist() + [0], [0,0,object_pose[-1][3]])
T_T0 = np.dot(invT0, T)
scale, shear, angles, trans, persp = tfm.decompose_matrix(T_T0)
vals.append(np.append(trans[0:2] * 1000,np.unwrap([angles[2]]) * 180 / np.pi))
# extract traj
if rep in xrange(1,901,30):
traj = []
for p in object_pose:
T = matrix_from_xyzrpy(p[1:3].tolist() + [0], [0,0,p[3]])
T_T0 = np.dot(invT0, T)
scale, shear, angles, trans, persp = tfm.decompose_matrix(T_T0)
traj.append(np.append([p[0]-object_pose[0][0]], np.append(trans[0:2] * 1000, np.unwrap([angles[2]]) * 180 / np.pi)) )
trajs.append(traj)
#ft_array[:][0] = ft_array[:][0] - ft_array[0][0]
#fts.append(ft_array)
#import pdb; pdb.set_trace()
(x,y,th)=zip(*(vals))
print 'covariance\n', np.cov(vals, rowvar=0)
print 'mean', np.mean(vals, axis = 0)
#from latexify import latexify; latexify(fig_width=3.39, fig_height=3.39*(sqrt(5)-1.0)/2.0*2, scale = 2)
f, ((ax1, ax2, ax3), (ax4, ax5, ax6), (ax7, ax8, ax9), (ax10, ax11, ax12)) = plt.subplots(4, 3)
n, bins, patches= ax1.hist(x, 200, normed=1, histtype='stepfilled',
facecolor='none', label='x', alpha=1)
ax1.set_title('Histogram of $\Delta x$')
n, bins, patches= ax2.hist(y, 200, normed=1, histtype='stepfilled',
facecolor='none', label='y', alpha=1)
ax2.set_title('Histogram of $\Delta y$')
n, bins, patches= ax3.hist(th, 200, normed=1, histtype='stepfilled',
facecolor='none', label='theta', alpha=1)
ax3.set_title('Histogram of $\Delta\\theta$')
ax4.scatter(x,y, color='k')
ax4.set_title('Scatter plot: $\Delta x$ versus $\Delta y$')
ax5.hist2d(x, y, bins=50)
ax5.set_title('2D Histogram: $\Delta x$ versus $\Delta y$')
meantraj = np.array(trajs[0])*0
#import pdb; pdb.set_trace()
for i in xrange(30):
lenn = min(len(meantraj), len(trajs[i]))
meantraj = np.array(meantraj[0:lenn]) + np.array(trajs[i][0:lenn]) / 30.0
#import pdb; pdb.set_trace()
lenn = len(meantraj)
(tm0,x0,y0,th0)=zip(*meantraj)
for i in xrange(30):
(tm,x,y,th)=zip(*(trajs[i]))
ax7.plot(tm[0:lenn], np.array(x[0:lenn])-np.array(x0))
ax8.plot(tm[0:lenn], np.array(y[0:lenn])-np.array(y0))
ax9.plot(tm[0:lenn], np.array(th[0:lenn])-np.array(th0))
ax7.set_title('$\Delta x$ over time')
ax8.set_title('$\Delta y$ over time')
ax9.set_title('$\Delta \\theta$ over time')
(tm,fx,fy,torq)=zip(*(fts[0]))
ax10.plot(tm, fx)
ax11.plot(tm, fy)
ax12.plot(tm, torq)
plt.tight_layout()
plt.savefig(figfname_png)
plt.savefig(figfname_pdf)
plt.show()
def plot(data, shape_id, figfname):
#data['tip_poses']
#data['ft_wrench']
#data['object_pose']
probe_radii = {'probe1' : 0.00626/2, 'probe2': 0.004745, 'probe3': 0.00475, 'probe4': 0.00475}
probe_radius = probe_radii['probe4']
fig, ax = plt.subplots()
fig.set_size_inches(7,7)
v = int(getfield_from_filename(os.path.basename(figfname), 'v'))
try:
a = int(getfield_from_filename(os.path.basename(figfname), 'a'))
except:
a = 0
if a!=0:
sub = int((2500.0**2) * 2 /(a**2))
if sub < 1: sub = 1
elif v!=-1:
sub = int(30*20 / (v)) # subsample rate
if sub < 1: sub = 1
tip_pose = data['tip_pose']
patches = []
# add the object as polygon
shape_db = ShapeDB()
shape = shape_db.shape_db[shape_id]['shape'] # shape of the objects presented as polygon.
shape_type = shape_db.shape_db[shape_id]['shape_type']
if shape_type == 'poly':
shape_polygon_3d = np.hstack((np.array(shape), np.zeros((len(shape), 1)), np.ones((len(shape), 1))))
elif shape_type == 'ellip':
shape = shape[0]
elif shape_type == 'polyapprox':
shape_polygon_3d = np.hstack((np.array(shape[0]), np.zeros((len(shape[0]), 1)), np.ones((len(shape[0]), 1))))
object_pose = data['object_pose']
if len(object_pose) > 0:
#invT0 = np.linalg.inv(matrix_from_xyzquat(object_pose[0][1:4], object_pose[0][4:8]))
invT0 = np.linalg.inv(matrix_from_xyzrpy(object_pose[0][1:3].tolist() + [0], [0,0,object_pose[0][3]]))
elif len(tip_pose) > 0:
invT0 = np.linalg.inv(matrix_from_xyzquat(tip_pose[0][1:3].tolist() +[0], [0,0,0,1]))
print 'object_pose', len(object_pose), 'tip_pose', len(tip_pose)
r = []
if len(object_pose) > 0:
r = (range(0, len(object_pose), sub)) + [len(object_pose)-1]
for i in r:
T = matrix_from_xyzrpy(object_pose[i][1:3].tolist() + [0], [0,0,object_pose[i][3]])
if i == 0:
alpha , fill = (0.3, True)
elif i == r[-1]:
alpha , fill = (0.6, True)
else:
alpha , fill = (0.6, False)
ec, fc = 'black','orangered'
if shape_type == 'poly' or shape_type == 'polyapprox':
shape_polygon_3d_world = np.dot(np.dot(invT0, T), shape_polygon_3d.T)
obj = mpatches.Polygon(shape_polygon_3d_world.T[:,0:2], closed=True, fc=fc, ec=ec, alpha=alpha, fill=fill, linewidth=1, linestyle='solid')
elif shape_type == 'ellip':
T_T0 = np.dot(invT0, T)
scale, shear, angles, trans, persp = tfm.decompose_matrix(T_T0)
obj = mpatches.Ellipse(trans[0:2], shape[0]*2, shape[1]*2, angle=angles[2], fc=fc, ec=ec, alpha=alpha, fill=fill, linewidth=1, linestyle='solid')
ax.add_patch(obj)
# add the probes as circle
r = []
if len(tip_pose) > 0:
r = (range(0, len(tip_pose), sub)) + [len(tip_pose)-1]
for i in r:
tip_pose_0 = np.dot(invT0, tip_pose[i][1:3].tolist()+[0,1])
if i == 0:
alpha , fill = (0.8, False)
elif i == r[-1]:
alpha , fill = (0.8, False)
else:
alpha , fill = (0.8, False)
circle = mpatches.Circle(tip_pose_0[0:2], probe_radius, color='black', alpha=alpha, fill=fill, linewidth=1, linestyle='solid')
ax.add_patch(circle)
# render it
plt.axis([-0.15, 0.15, -0.15, 0.15])
plt.axis('off')
if figfname is not None:
plt.savefig(figfname)
def wait_for_odom_angle(timeout=None):
odom = rospy.wait_for_message("odom", Odometry, timeout=timeout)
pose = numpify(odom.pose.pose)
_, _, angles, _, _ = decompose_matrix(pose)
theta = angles[2] * 180 / 3.14159
return theta
def Ts_to_tfs(Ts):
calibration_tfs = []
for transform in Ts:
_, _, rotation, translation, _ = transformations.decompose_matrix(transform)
calibration_tfs.append(np.append(translation, rotation))
return calibration_tfs
