def _load_mvbbs_simulated(self):
try:
f = open(self.filename_simulated)
reader = csv.reader(f)
self.db_simulated = {}
for row in reader:
object_dims = tuple(
[float(v) for i, v in enumerate(row) if i in range(3)])
t = [float(v) for i, v in enumerate(row) if i in range(3, 6)]
q = [float(row[9])] + [
float(v) for i, v in enumerate(row) if i in range(6, 9)
]
T = np.array(se3.homogeneous((so3.from_quaternion(q), t)))
t = [float(v) for i, v in enumerate(row) if i in range(10, 13)]
q = [float(row[16])] + [
float(v) for i, v in enumerate(row) if i in range(13, 16)
]
h_T_o = np.array(se3.homogeneous((so3.from_quaternion(q), t)))
q_hand = [
float(v) for i, v in enumerate(row)
if i in range(17, 17 + self.n_dofs)
]
c_p = [
float(v) for i, v in enumerate(row)
if i in range(17 + self.n_dofs, 17 + self.n_dofs +
3 * self.n_l)
]
c_f = [
float(v) for i, v in enumerate(row)
if i in range(17 + self.n_dofs + 3 * self.n_l, 17 +
self.n_dofs + 9 * self.n_l)
]
if tuple(object_dims) not in self.db_simulated:
self.db_simulated[tuple(object_dims)] = []
obj_pose_q_c_p_c_f = {
'T': T,
'h_T_o': h_T_o,
'q': q_hand,
'c_p': c_p,
'c_f': c_f
}
self.db_simulated[tuple(object_dims)].append(
obj_pose_q_c_p_c_f)
except:
pass
def so3_grid(N):
"""Returns a nx.Graph describing a grid on SO3 with resolution N.
The resulting graph has ~4N^3 nodes
The node attribute 'params' gives the so3 element.
"""
assert N >= 1
G = nx.Graph()
R0 = so3.from_quaternion(vectorops.unit((1, 1, 1, 1)))
namemap = dict()
for ax in range(4):
for i in xrange(N + 1):
u = 2 * float(i) / N - 1.0
u = math.tan(u * math.pi * 0.25)
for j in xrange(N + 1):
v = 2 * float(j) / N - 1.0
v = math.tan(v * math.pi * 0.25)
for k in xrange(N + 1):
w = 2 * float(k) / N - 1.0
w = math.tan(w * math.pi * 0.25)
idx = [i, j, k]
idx = idx[:ax] + [N] + idx[ax:]
idx = tuple(idx)
if idx in namemap:
continue
else:
namemap[idx] = idx
flipidx = tuple([N - x for x in idx])
namemap[flipidx] = idx
q = [u, v, w]
q = q[:ax] + [1.0] + q[ax:]
#print idx,"->",q
q = vectorops.unit(q)
R = so3.mul(so3.inv(R0), so3.from_quaternion(q))
G.add_node(idx, params=R)
for ax in range(4):
for i in xrange(N + 1):
for j in xrange(N + 1):
for k in xrange(N + 1):
baseidx = [i, j, k]
idx = baseidx[:ax] + [N] + baseidx[ax:]
idx = tuple(idx)
for n in range(3):
nidx = baseidx[:]
nidx[n] += 1
if nidx[n] > N: continue
nidx = nidx[:ax] + [N] + nidx[ax:]
nidx = tuple(nidx)
G.add_edge(namemap[idx], namemap[nidx])
return G
def load_rgbd_dataset(dataset):
"""Returns a list of RGBDScans from a folder"""
ground_truth_t,ground_truth_transforms = _load_time_stamped_file(dataset+'/groundtruth.txt')
ground_truth_transforms = [[float(v) for v in T] for T in ground_truth_transforms]
#convert to Klamp't SE3 elements
for i,trans_quat in enumerate(ground_truth_transforms):
qx,qy,qz,qw = trans_quat[3:]
ground_truth_transforms[i] = (so3.from_quaternion((qw,qx,qy,qz)),trans_quat[:3])
ground_truth_path = SE3Trajectory(ground_truth_t,ground_truth_transforms)
rgb_t,rgb_files = _load_time_stamped_file(dataset+'/rgb.txt')
depth_t,depth_files = _load_time_stamped_file(dataset+'/depth.txt')
depth_t = np.array(depth_t)
scans = []
last_ind = -1
for t,file in zip(rgb_t,rgb_files):
im = Image.open(dataset+'/'+file)
rgb_array = np.asarray(im)
ind = np.abs(depth_t-t).argmin()
if ind > last_ind+1:
ind = last_ind+1
last_ind = ind
#print("Time",t,"Depth index",ind,"Distance",np.abs(depth_t-t)[ind])
depth_file = depth_files[ind]
im = Image.open(dataset+'/'+depth_file)
depth_array = np.asarray(im)
T = ground_truth_path.eval_se3(t)
scans.append(RGBDScan(rgb_array,depth_array,timestamp=t,T_ground_truth=T))
scans[-1].camera = kinect_factory_calib
return scans
def test_rpy(self):
R = so3.from_quaternion(
(-4.32978e-17, -0.707107, 4.32978e-17, 0.707107))
r, p, y = so3.rpy(R)
self.assertAlmostEqual(r, 0.0)
self.assertAlmostEqual(p, 1.5707963267948966)
self.assertAlmostEqual(y, 3.141592653589793)
def spherical_linear_interpolation_fromR(R0, Rf, h):
# See https://en.wikipedia.org/wiki/Slerp
q0 = np.array(so3.quaternion(R0))
qf = np.array(so3.quaternion(Rf))
v0 = np.array(MathUtils.normalize(q0))
vf = np.array(MathUtils.normalize(qf))
dot = np.dot(v0, vf)
if dot < 0.0:
vf = -vf
dot = -dot
# TODO: Is this causing non rotation matrixes?
dot_threshold = 0.995
if dot > dot_threshold:
# print(f"WARNING: dot={dot} > threshold={dot_threshold}\t linearly interprolating")
# If results are close, calculate new result linearly
result = v0 + h * (vf - v0)
result_as_list = result.tolist()
q = MathUtils.normalize(result_as_list)
else:
theta_0 = np.arccos(dot)
theta = theta_0 * h
sin_theta = np.sin(theta)
sin_theta_0 = np.sin(theta_0)
s0 = np.cos(theta) - dot * sin_theta / sin_theta_0
s1 = sin_theta / sin_theta_0
q = (s0 * v0) + (s1 * vf)
return so3.from_quaternion(q)
def rand_rotation():
q = [
random.gauss(0, 1),
random.gauss(0, 1),
random.gauss(0, 1),
random.gauss(0, 1)
]
q = vectorops.unit(q)
return so3.from_quaternion(q)
def do_lookup(frame,parent):
try:
(trans,rot) = listener.lookupTransform(frame, parent, rospy.Time(0))
return (so3.from_quaternion((rot[3],rot[0],rot[1],rot[2])),trans)
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
if onerror == 'print':
print("listen_tf: Error looking up frame",frame)
elif onerror == 'raise':
raise
return None
def test_getObjectPhalanxMeanContactPoint(self):
world = WorldModel()
world.loadElement(abspath + '/../data/terrains/plane.env')
obj = make_box(world, 0.03, 0.05, 0.1)
R, t = obj.getTransform()
obj.setTransform(R, [0, 0, 0.1 / 2.])
p = [0.015, -0.025, 0.1]
q = [3.7494e-33, 6.12323e-17, 1.0, -6.12323e-17]
o_T_p = np.array(se3.homogeneous((so3.from_quaternion(q), p)))
w_T_o = np.array(se3.homogeneous(obj.getTransform()))
p = (-0.03, -0.12, -0.013 + 0.02
) # -0.013 + 0.02, from working exp at commit 98305499
R = (0.0, 0.0, 1.0, 1.0, 0.0, 0.0, 0.0, 1.0, 0.0)
p_T_h = np.array(se3.homogeneous((R, p)))
w_T_h_des = w_T_o.dot(o_T_p).dot(p_T_h)
w_T_h_des_se3 = se3.from_homogeneous(w_T_h_des)
robot = make_moving_base_robot('soft_hand', world)
set_moving_base_xform(robot, w_T_h_des_se3[0], w_T_h_des_se3[1])
sim = SimpleSimulator(world)
# setup the preshrink
visPreshrink = False # turn this to true if you want to see the "shrunken" models used for collision detection
for l in range(robot.numLinks()):
sim.body(robot.link(l)).setCollisionPreshrink(visPreshrink)
for l in range(world.numRigidObjects()):
sim.body(world.rigidObject(l)).setCollisionPreshrink(visPreshrink)
hand = plugins.soft_hand.HandEmulator(sim, 0, 6, 6)
sim.addEmulator(0, hand)
links_to_check = np.array([
3, 4, 6, 8, 10, 13, 15, 17, 20, 22, 24, 27, 29, 31, 33, 35, 37
]) + 6
#vis.add("world", world)
#vis.show()
for i in range(100):
#vis.lock()
hand.setCommand([np.min((i / 10., 1.0))])
sim.simulate(0.01)
sim.updateWorld()
#vis.unlock()
c_p, c_f = getObjectPhalanxMeanContactPoint(
sim, obj, robot, links_to_check)
c_p_all, c_f_all = getObjectPhalanxMeanContactPoint(
sim, obj, robot)
if i == 0:
self.assertEqual(countContactPoints(c_p), 0)
self.assertEqual(countContactPoints(c_p_all), 0)
elif i == 99:
self.assertTrue(countContactPoints(c_p) > 0)
self.assertTrue(countContactPoints(c_p_all) > 0)
def _load_mvbbs(self):
try:
f = open(self.filename)
self.db = {}
reader = csv.reader(f)
for row in reader:
object_dims = tuple(
[float(v) for i, v in enumerate(row) if i in range(3)])
t = [float(v) for i, v in enumerate(row) if i in range(3, 6)]
q = [float(row[9])] + [
float(v) for i, v in enumerate(row) if i in range(6, 9)
]
T = (so3.from_quaternion(q), t)
if tuple(object_dims) not in self.db:
self.db[object_dims] = []
self.db[object_dims].append(np.array(se3.homogeneous(T)))
except:
pass
def transform_average(Ts):
t = vectorops.div(vectorops.add(*[T[1] for T in Ts]), len(Ts))
qs = [so3.quaternion(T[0]) for T in Ts]
q = vectorops.div(vectorops.add(*qs), len(Ts))
return (so3.from_quaternion(q), t)
def from_Quaternion(ros_q):
"""From ROS Quaternion to Klamp't so3 element"""
q = [ros_q.w, ros_q.x, ros_q.y, ros_q.z]
return so3.from_quaternion(q)
def so3_staggered_grid(N):
"""Returns a nx.Graph describing a staggered grid on SO3 with resolution N.
The resulting graph has ~8N^3 nodes
The node attribute 'params' gives the so3 element.
"""
import itertools
assert N >= 1
G = nx.Graph()
R0 = so3.from_quaternion(vectorops.unit((1, 1, 1, 1)))
#R0 = so3.identity()
namemap = dict()
for ax in range(4):
for i in xrange(N + 1):
u = 2 * float(i) / N - 1.0
u = math.tan(u * math.pi * 0.25)
u2 = 2 * float(i + 0.5) / N - 1.0
u2 = math.tan(u2 * math.pi * 0.25)
for j in xrange(N + 1):
v = 2 * float(j) / N - 1.0
v = math.tan(v * math.pi * 0.25)
v2 = 2 * float(j + 0.5) / N - 1.0
v2 = math.tan(v2 * math.pi * 0.25)
for k in xrange(N + 1):
w = 2 * float(k) / N - 1.0
w = math.tan(w * math.pi * 0.25)
w2 = 2 * float(k + 0.5) / N - 1.0
w2 = math.tan(w2 * math.pi * 0.25)
idx = [i, j, k]
idx = idx[:ax] + [N] + idx[ax:]
idx = tuple(idx)
if idx in namemap:
pass
else:
namemap[idx] = idx
flipidx = tuple([N - x for x in idx])
namemap[flipidx] = idx
q = [u, v, w]
q = q[:ax] + [1.0] + q[ax:]
q = vectorops.unit(q)
R = so3.mul(so3.inv(R0), so3.from_quaternion(q))
G.add_node(idx, params=R)
if i < N and j < N and k < N:
#add staggered point
sidx = [i + 0.5, j + 0.5, k + 0.5]
sidx = sidx[:ax] + [N] + sidx[ax:]
sidx = tuple(sidx)
sfidx = tuple([N - x for x in sidx])
namemap[sidx] = sidx
namemap[sfidx] = sidx
q = [u2, v2, w2]
q = q[:ax] + [1.0] + q[ax:]
q = vectorops.unit(q)
R = so3.mul(so3.inv(R0), so3.from_quaternion(q))
G.add_node(sidx, params=R)
for ax in range(4):
for i in xrange(N + 1):
for j in xrange(N + 1):
for k in xrange(N + 1):
baseidx = [i, j, k]
idx = baseidx[:ax] + [N] + baseidx[ax:]
idx = tuple(idx)
for n in range(3):
nidx = baseidx[:]
nidx[n] += 1
if nidx[n] > N: continue
nidx = nidx[:ax] + [N] + nidx[ax:]
nidx = tuple(nidx)
G.add_edge(namemap[idx], namemap[nidx])
#edges between staggered point and grid points
baseidx = [i + 0.5, j + 0.5, k + 0.5]
if any(v > N for v in baseidx): continue
idx = baseidx[:ax] + [N] + baseidx[ax:]
idx = tuple(idx)
for ofs in itertools.product(*[(-0.5, 0.5)] * 3):
nidx = vectorops.add(baseidx, ofs)
nidx = [int(x) for x in nidx]
nidx = nidx[:ax] + [N] + nidx[ax:]
nidx = tuple(nidx)
#print "Stagger-grid edge",idx,nidx
G.add_edge(namemap[idx], namemap[nidx])
#edges between staggered points -- if it goes beyond face,
#go to the adjacent face
for n in range(3):
nidx = baseidx[:]
nidx[n] += 1
if nidx[n] > N:
#swap face
nidx[n] = N
nidx = nidx[:ax] + [N - 0.5] + nidx[ax:]
nidx = tuple(nidx)
#if not G.has_edge(namemap[idx],namemap[nidx]):
# print "Stagger-stagger edge",idx,nidx,"swap face"
else:
nidx = nidx[:ax] + [N] + nidx[ax:]
nidx = tuple(nidx)
#if not G.has_edge(namemap[idx],namemap[nidx]):
# print "Stagger-stagger edge",idx,nidx
G.add_edge(namemap[idx], namemap[nidx])
return G
def test_rpy(self):
R = so3.from_quaternion((-4.32978e-17, -0.707107,4.32978e-17,0.707107))
r,p,y = so3.rpy(R)
self.assertAlmostEqual(r,0.0)
self.assertAlmostEqual(p,1.5707963267948966)
self.assertAlmostEqual(y,3.141592653589793)
def normalize_rotation(R):
"""Given a matrix close to a proper (orthogonal) rotation matrix,
returns a true orthogonal matrix."""
q = so3.quaternion(R) #normalizes
return so3.from_quaternion(q)
import threading
from sspp.service import Service
from sspp.topic import TopicListener
import asyncore
# parameters for initial robot pose
q_init_left = [-0.08897088559570313, -0.9675583808532715, -1.2034079267211915, 1.7132575355041506, 0.6776360122741699, 1.0166457660095216, -0.5065971546203614]
q_init_right = [0.08897088559570313, -0.9675583808532715, 1.2034079267211915, 1.7132575355041506, -0.6776360122741699, 1.0166457660095216, 0.5065971546203614]
q_tuckarm_left = [0.58897088559570313, -0.9675583808532715, -1.2034079267211915, 1.7132575355041506, 0.6776360122741699, 1.0166457660095216, -0.5065971546203614]
q_tuckarm_right = [-0.58897088559570313, -0.9675583808532715, 1.2034079267211915, 1.7132575355041506, -0.6776360122741699, 1.0166457660095216, 0.5065971546203614]
q = (0.564775,0.425383,0.426796,0.563848)
R1 = so3.from_quaternion(q)
t1 = (0.227677,0.0916972,0.0974174)
kinect_frame_to_pedestal = (R1, t1)
#Configuration variable: where's the state server?
system_state_addr = EbolabotSystemConfig.getdefault_ip('state_server_computer_ip',('localhost',4568))
#imaging stuff
try:
from PIL import Image
except ImportError, err:
import Image
class GLTexture:
B = so3.from_quaternion(A) # [a11, a21, a31, ...]
C = so3.matrix(B)
T = np.eye(4)
T[:3, :3] = C
T[:3, 3] = t
out_data.append(list(T[0]) + list(T[1]) + list(T[2]) + list(T[3]))
"""
TT = [0.492127,0.003738762,0.08270439,-0.02360799,0.7022632,-0.3517123,-0.5801101,-0.2158636] # from advio-04
TT = [0.514813,-0.005759389,-0.09049783,-0.001374606,0.64807,-0.264358,0.6264483,0.343049] # a complete line from advio-23
A = TT[4: 8]
B = so3.from_quaternion(A)
print(B)
C = so3.matrix(B)
print(C)
t = TT[1: 4]
T = np.eye(4)
T[:3, :3] = C
T[:3, 3] = t
print(T)
print([list(T[0]), list(T[1]), list(T[2]), list(T[3])])
print(np.array([list(T[0]), list(T[1]), list(T[2]), list(T[3])]))
print(list(T[0]) + list(T[1]) + list(T[2]) + list(T[3]))
