def get_rotation_matrix(self):
q1 = Quat(axis=[1, 0, 0], degrees=self.rotation[0])
q2 = Quat(axis=[0, 1, 0], degrees=self.rotation[1])
q3 = Quat(axis=[0, 0, 1], degrees=self.rotation[2])
q = q3*q2*q1
return q.rotation_matrix
def plot_corrected_linear_acceleration(bag):
x, y, z = [], [], []
x_, y_, z_ = [], [], []
i = 0
for topic, msg, t in bag.read_messages(topics="/imu0/data"):
q = Quat(msg.orientation.w, msg.orientation.x, msg.orientation.y,
msg.orientation.z)
a_ = q.rotate((msg.linear_acceleration.x, msg.linear_acceleration.y,
msg.linear_acceleration.z))
x.append(msg.linear_acceleration.x)
y.append(msg.linear_acceleration.y)
z.append(msg.linear_acceleration.z)
x_.append(a_[0])
y_.append(a_[1])
z_.append(a_[2])
if i == 2000:
print x_[i], y_[i], z_[i]
i += 1
plt.plot(x, label='a_x')
plt.plot(y, label='a_y')
plt.plot(z, label='a_z')
plt.plot(x_, label='corr a_x')
plt.plot(y_, label='corr a_y')
plt.plot(z_, label='corr a_z')
plt.legend()
plt.show()
def get_matrix_from_quat(quat, real_first=False): # TODO: do this in a way which doesn't require an additional library (maybe pybullet?) if not real_first: # This inversion means that the quat should be given in [i,j,k,r]. This is to match pybullet and ros. quat = [quat[3]] + quat[:3] quatObj = Quat(quat) return quatObj.rotation_matrix.tolist()
def plot_corrected_angular_velocity(bag):
x, y, z = [], [], []
x_, y_, z_ = [], [], []
imu_2_footprint = np.array((0, -1.03, .73))
dt = 1. / 250 #s
i = 0
prev_ang_vel = None
velocity = np.array((0, 0, 0), dtype=np.float64)
frame_ori = Quat(
matrix=np.array([[0, 1, 0], [1, 0, 0], [0, 0, -1]], dtype=np.float64))
for topic, msg, t in bag.read_messages(
topics="/imu/data",
start_time=rospy.Time(bag.get_start_time()) + rospy.Duration(60)):
curr_ori = Quat(msg.orientation.w, msg.orientation.x,
msg.orientation.y, msg.orientation.z)
curr_ang_vel = np.asarray(
(msg.angular_velocity.x, msg.angular_velocity.y,
msg.angular_velocity.z))
curr_time = msg.header.stamp.to_sec()
if prev_ang_vel is not None:
curr_accel = curr_ori.rotate(
np.asarray(
(msg.linear_acceleration.x, msg.linear_acceleration.y,
msg.linear_acceleration.z)))
'''ang_accel = (curr_ori.rotate(curr_ang_vel) - prev_ori.rotate(prev_ang_vel))/dt
accel_footprint = curr_accel \
+ np.cross(np.cross(curr_ori.rotate(curr_ang_vel), curr_ori.rotate(imu_2_footprint)), curr_ori.rotate(curr_ang_vel)) \
+ np.cross(curr_ori.rotate(imu_2_footprint), ang_accel)'''
velocity += (curr_accel - np.array(
(0, 0, 9.8))) * (curr_time - prev_time)
# print velocity
x_.append(velocity[0])
y_.append(velocity[1])
z_.append(velocity[2])
prev_time = curr_time
prev_ang_vel = curr_ang_vel
prev_ori = curr_ori
#i += 1
plt.plot(x_, label='footprint a_x')
plt.plot(y_, label='footprint a_y')
plt.plot(z_, label='footprint a_z')
plt.legend()
plt.show()
def _do_fk(config, fk_func, real_first=False): pose = fk_func(config) pos = pose[0] rot = pose[1] # 3x3 rotation matrix quat = Quat(matrix=np.array(rot)) quat = quat if quat.real >= 0 else -quat # solves q and -q being same rotation quat = quat.unit quat = quat.elements.tolist() # switch from [r, i, j, k] to [i, j, k, r] if not real_first: quat = quat[1:] + [quat[0]] return pos, quat
3] == 0.).astype('uint8'),
inpaintRadius=3,
flags=cv2.INPAINT_NS)
return img
def visualize(img, title=""):
cv2.imshow(title + '_rgb', img[:, :, 0:3].astype('uint8'))
cv2.imshow(title + '_depth', (1000 * img[:, :, 3]).astype('uint8'))
if __name__ == '__main__':
# Camera movement
dx = [-0.1502, -0.2313, 0.465]
dq = Quat(axis=[0, 0, 1], angle=np.pi * (9. / 8.)).elements
#dx = [0.,0., 0.05]
#dq = Quat(axis=[0,0,1], angle=0).elements
dxs = np.stack([dx, dx], axis=0)
dqs = np.stack([dq, dq], axis=0)
# Load images
data_dir = "/home/aswang/Desktop/efort_test/"
img1 = load_image(data_dir + "a0_rgb.png", data_dir + "a0_depth.png")
#img2 = load_image("test_rgb2.png", data_dir"1_depth.png")
#imgs = np.stack([img1, img2], axis=0)
# Transform
start = time.time()
img = np.empty([480, 640, 4]).astype('float32')
img[:, :, 0:3] = cv2.imread(rgb_file, flags=cv2.IMREAD_COLOR)
img[:, :, 3] = cv2.imread(depth_file, flags=cv2.IMREAD_UNCHANGED) / 1000.
return img
def visualize(img, title=""):
cv2.imshow(title + '_rgb', img[:, :, 0:3].astype('uint8'))
#cv2.imshow(title+'_depth', (1000*img[:,:,3]).astype('uint8'))
if __name__ == '__main__':
# Camera movement
dx = [0.03, -0.002, 0.465]
dq = Quat(axis=[0, 0, 1], angle=0.4).elements
dxs = np.stack([dx, dx], axis=0)
dqs = np.stack([dq, dq], axis=0)
# Load images
img1 = load_image("test_rgb1.png", "test_depth1.png")
img2 = load_image("test_rgb2.png", "test_depth2.png")
imgs = np.stack([img1, img2], axis=0)
# Transform
start = time.time()
transformer = PlanarHomographyTransformer(K)
#new_img = transformer.transform(img1, dx, dq)
new_imgs = transformer.transform_batch(imgs, dxs, dqs, gpu=False)
# from ur_control.compliant_controller import CompliantController
from ur3e_openai.robot_envs.utils import get_conical_helix_trajectory
path = Path()
inital = [
-0.0012867963980835793, -0.35302556809186536, 0.4085084665890285,
-0.0498093375695251, -0.6912765989420319, 0.7208648458793617,
0.0030931571809757674
]
target = [
-0.003197180674706364, -0.37576393689660575, 0.45137435818768146,
-0.0021967960033876664, -0.6684438218043409, 0.7437414872148852,
-0.005160559496475987
]
target_q = Quat(np.roll(target[3:], 1))
steps = 300
p2 = np.zeros(3)
p1 = target_q.rotate(np.array(inital[:3]) - target[:3])
traj = get_conical_helix_trajectory(p1, p2, steps, 3.0)
traj = np.apply_along_axis(target_q.rotate, 1, traj)
traj = traj + target[:3]
rospy.init_node('path_node')
path = Path()
h = Header()