def __init__(self):
self.reset()
self.color = [-1, -1, -1]
#TODO: fill this in with your own camera model, if you wish
self.Tsensor = None
cameraRot = [0, -1, 0, 0, 0, -1, 1, 0, 0]
self.w, self.h = 320, 240
self.fov = 90
self.dmax = 7
if event == 'A':
#at goal post, pointing a bit up and to the left
self.Tsensor = (so3.mul(
so3.rotation([0, 0, 1], 0.20),
so3.mul(so3.rotation([0, -1, 0], 0.25),
cameraRot)), [-2.55, -1.1, 0.25])
elif event == 'B':
#on ground near robot, pointing up and slightly to the left
self.Tsensor = (so3.mul(
so3.rotation([1, 0, 0], -0.10),
so3.mul(so3.rotation([0, -1, 0], math.radians(90)),
cameraRot)), [-1.5, -0.5, 0.25])
self.w = 640
self.h = 480
self.dmax = 10
else:
#on ground near robot, pointing to the right
self.Tsensor = (cameraRot, [-1.5, -0.5, 0.25])
self.dt = 0.02
return
def __init__(self):
cameraRot = [0, -1, 0, 0, 0, -1, 1, 0, 0]
#at goal post, pointing a bit up and to the left
self.Tsensor = (so3.mul(
so3.rotation([0, 0, 1], 0.20),
so3.mul(so3.rotation([0, -1, 0], 0.25),
cameraRot)), [-2.55, -1.1, 0.25])
self.T_r = np.matrix(self.Tsensor[0]).reshape((3, 3)).T
self.T_t = np.matrix(self.Tsensor[1]).reshape((3, 1))
self.fov = 90
self.w, self.h = 320, 240
self.dmax = 5
self.dt = 0.02
self.f = (0.5 * self.w) / np.tan(self.fov / 2.0 / 180 * np.pi)
# Kalman Filter parameters
self.A = np.vstack((np.hstack((np.zeros(
(3, 3)), np.eye(3))), np.zeros((3, 6))))
self.dyn_noise = np.vstack((np.hstack(
(np.eye(3) * (0.5 * self.dt**2 * 0.008)**2, np.zeros(
(3, 3)))), np.hstack((np.zeros(
(3, 3)), np.eye(3) * 0.008)))) # covariance
self.F = np.eye(self.A.shape[0]) + self.A * self.dt
self.g = np.vstack((np.zeros((5, 1)), np.matrix([-GRAVITY]))) * self.dt
self.H_jaccobian = None # use calJaccobian to calculate this when there is some estimated state to use
self.obs_noise = np.diag([0.25, 0.25, 0.5, 0.5])
# prior
self.previous_estimates = dict()
self.previous_cov = dict()
return
def sample_object_pose_table(obj, stable_fs, bmin, bmax):
"""Samples a transform of the object so that it lies on in the given
bounding box bmin,bmax.
Args:
obj (RigidObjectModel)
stable_fs (list of lists): giving the stable faces of the object,
as in MP2.
bmin,bmax (3-vectors): the bounding box of the area in which the
objects should lie.
"""
table_height = bmin[2] + 0.001
face = random.choice(stable_fs)
normal = np.cross(face[1] - face[0], face[2] - face[0])
normal = normal / np.linalg.norm(normal)
centroid = np.sum(face, axis=0) / len(face)
x = random.uniform(bmin[0], bmax[0])
y = random.uniform(bmin[1], bmax[1])
z = table_height + np.dot(centroid, normal)
Rnormal = so3.canonical((-normal).tolist())
Rx = so3.rotation((1, 0, 0), random.uniform(0, math.pi * 2))
Rxz = so3.rotation((0, 1, 0), -math.pi * 0.5)
R = so3.mul(Rxz, so3.mul(Rx, so3.inv(Rnormal)))
#R*com + t = [x,y,_]
t = vectorops.sub([x, y, z], so3.apply(R, obj.getMass().getCom()))
t[2] = z
obj.setTransform(R, t)
def euler_angle_to_rotation(ea,convention='zyx'):
"""Converts an euler angle representation to a rotation matrix.
Can use arbitrary axes specified by the convention
arguments (default is 'zyx', or roll-pitch-yaw euler angles). Any
3-letter combination of 'x', 'y', and 'z' are accepted.
"""
axis_names_to_vectors = dict([('x',(1,0,0)),('y',(0,1,0)),('z',(0,0,1))])
axis0,axis1,axis2=convention
R0 = so3.rotation(axis_names_to_vectors[axis0],ea[0])
R1 = so3.rotation(axis_names_to_vectors[axis1],ea[1])
R2 = so3.rotation(axis_names_to_vectors[axis2],ea[2])
return so3.mul(R0,so3.mul(R1,R2))
def main():
w = WorldModel()
w.readFile("../data/robots/kinova_gen3_7dof.urdf")
robot = w.robot(0)
#put a "pen" geometry on the end effector
gpen = Geometry3D()
res = gpen.loadFile("../data/objects/cylinder.off")
assert res
gpen.scale(0.01,0.01,0.15)
gpen.rotate(so3.rotation((0,1,0),math.pi/2))
robot.link(7).geometry().setCurrentTransform(*se3.identity())
gpen.transform(*robot.link(8).getParentTransform())
robot.link(7).geometry().set(merge_geometry.merge_triangle_meshes(gpen,robot.link(7).geometry()))
robot.setConfig(robot.getConfig())
trajs = get_paths()
#place on a reasonable height and offset
tableh = 0.1
for traj in trajs:
for m in traj.milestones:
m[0] = m[0]*0.4 + 0.35
m[1] = m[1]*0.4
m[2] = tableh
if len(m) == 6:
m[3] *= 0.4
m[4] *= 0.4
return run_cartesian(w,trajs)
def __init__(self):
self.reset()
#TODO: fill this in with your own camera model, if you wish
self.Tsensor = None
cameraRot = [0, -1, 0, 0, 0, -1, 1, 0, 0]
self.w, self.h = 320, 240
self.fov = 90
self.dmax = 5
self.dt = 0.02
if event == 'A':
#at goal post, pointing a bit up and to the left
self.Tsensor = (so3.mul(
so3.rotation([0, 0, 1], 0.20),
so3.mul(so3.rotation([0, -1, 0], 0.25),
cameraRot)), [-2.55, -1.1, 0.25])
# Kalman Filter parameters
self.A = np.vstack((np.hstack((np.zeros(
(3, 3)), np.eye(3))), np.zeros((3, 6))))
self.dyn_noise = np.vstack((np.hstack(
(np.eye(3) * (0.5 * self.dt**2 * 0.008)**2, np.zeros(
(3, 3)))), np.hstack((np.zeros(
(3, 3)), np.eye(3) * 0.008)))) # covariance
self.F = np.eye(self.A.shape[0]) + self.A * self.dt
self.g = np.vstack((np.zeros(
(5, 1)), np.matrix([-GRAVITY]))) * self.dt
self.H_jaccobian = None # use calJaccobian to calculate this when there is some estimated state to use
self.obs_noise = np.diag([0.25, 0.25, 0.5, 0.5])
elif event == 'B':
#on ground near robot, pointing up and slightly to the left
self.Tsensor = (so3.mul(
so3.rotation([1, 0, 0], -0.10),
so3.mul(so3.rotation([0, -1, 0], math.radians(90)),
cameraRot)), [-1.5, -0.5, 0.25])
self.w = 640
self.h = 480
self.dmax = 10
else:
#on ground near robot, pointing to the right
self.Tsensor = (cameraRot, [-1.5, -0.5, 0.25])
self.T_r = np.matrix(self.Tsensor[0]).reshape((3, 3)).T
self.T_t = np.matrix(self.Tsensor[1]).reshape((3, 1))
self.f = (0.5 * self.w) / np.tan(self.fov / 2.0 / 180 * np.pi)
# prior
self.previous_estimates = dict()
self.previous_cov = dict()
return
def interpolate_rotation(R1, R2, u):
"""Interpolate linearly between the two rotations R1 and R2.
TODO: the current implementation doesn't work properly. Why? """
m1 = so3.moment(R1)
m2 = so3.moment(R2)
mu = interpolate_linear(m1, m2, u)
angle = vectorops.norm(mu)
axis = vectorops.div(mu, angle)
return so3.rotation(axis, angle)
def getWall(dimX, dimY, dimZ, pos = [0, 0, 0], rotZ = 0):
## Get the wall geometry
wall = Geometry3D()
wall.loadFile("cube.off") # The wall is based upon cube primitive of unit dimension
## Not sure why the scaling is done through the rotation matrix, works though!
#wall.transform([dimX, 0, 0, 0, dimY, 0, 0, 0, dimZ], pos)
wall.scale(dimX, dimY, dimZ)
rotMat = so3.rotation((0, 0, 1), math.radians(rotZ))
wall.transform(rotMat, pos)
return wall
def xy_randomize(obj,bmin,bmax): R,t = obj.getTransform() obmin,obmax = obj.geometry().getBB() w = 0.5*(obmax[0]-obmin[0]) h = 0.5*(obmax[1]-obmin[1]) correction = max(w,h) R = so3.mul(so3.rotation([0,0,1],random.uniform(0,math.pi*2)),R) t[0] = random.uniform(bmin[0]+correction,bmax[0]-correction) t[1] = random.uniform(bmin[1]+correction,bmax[1]-correction) obj.setTransform(R,t)
def xy_randomize(obj,bmin,bmax):
"""Randomizes the xy position and z orientation of an object inside of a bounding box bmin->bmax.
Assumes the bounding box is large enough to hold the object in any orientation.
"""
R,t = obj.getTransform()
R = so3.mul(so3.rotation([0,0,1],random.uniform(0,math.pi*2)),R)
obj.setTransform(R,t)
obmin,obmax = obj.geometry().getBB()
t[0] = random.uniform(bmin[0]-obmin[0],bmax[0]-obmax[0])
t[1] = random.uniform(bmin[1]-obmin[1],bmax[1]-obmax[1])
obj.setTransform(R,t)
def xy_randomize(obj,bmin,bmax):
"""Randomizes the xy position and z orientation of an object inside of a bounding box bmin->bmax.
Assumes the bounding box is large enough to hold the object in any orientation.
"""
R,t = obj.getTransform()
R = so3.mul(so3.rotation([0,0,1],random.uniform(0,math.pi*2)),R)
obj.setTransform(R,t)
obmin,obmax = obj.geometry().getBB()
t[0] = random.uniform(bmin[0]-obmin[0],bmax[0]-obmax[0])
t[1] = random.uniform(bmin[1]-obmin[1],bmax[1]-obmax[1])
obj.setTransform(R,t)
def euler_zyx_mat(theta):
"""For the zyx euler angles theta=(rz,ry,rx), produces a matrix A such that
A*dtheta is the angular velocities when dtheta is the rate of change of the
euler angles"""
eu = [0, 0, 1]
ev = [0, 1, 0]
ew = [1, 0, 0]
Ru = so3.rotation([0, 0, 1], theta[0])
Rv = so3.rotation([0, 1, 0], theta[1])
col1 = eu
col2 = so3.apply(Ru, ev)
col3 = so3.apply(Ru, so3.apply(Rv, ew))
#col1 = [0,0,1]
#col2 = [c0 -s0 0] [0] = [-s0]
# [s0 c0 0]*[1] [c0 ]
# [0 0 1] [0] [0 ]
#col3 = Ru*[c1 0 s1] [1] = Ru*[c1 ] = [c1c0]
# [0 1 0 ]*[0] [0 ] [c1s0]
# [-s1 0 c1] [0] [-s1] [-s1 ]
#A = [ 0 -s0 c1c0]
# [ 0 c0 c1s0]
# [ 1 0 -s1 ]
#return [col1[2], col2[2], col3[2], col1[1], col2[1], col3[1], col1[0], col2[0], col3[0]]
phi = theta[0]
tht = theta[1]
psi = theta[2]
cphi = math.cos(phi)
sphi = math.sin(phi)
ctht = math.cos(tht)
stht = math.sin(tht)
cpsi = math.cos(psi)
spsi = math.sin(psi)
rotMat = [
cphi * ctht, sphi * cpsi + cphi * stht * spsi,
sphi * spsi - cphi * stht * cpsi, -sphi * ctht,
cphi * cpsi - sphi * stht * spsi, cphi * spsi + sphi * stht * cpsi,
stht, -spsi * ctht, cpsi * ctht
]
return rotMat
def euler_zyx_moments(theta):
"""For the zyx euler angles theta=(rz,ry,rx), produces a matrix A such that
A*dtheta is the angular velocities when dtheta is the rate of change of the
euler angles"""
eu = [0, 0, 1]
ev = [0, 1, 0]
ew = [1, 0, 0]
Ru = so3.rotation([0, 0, 1], theta[0])
Rv = so3.rotation([0, 1, 0], theta[1])
col1 = eu
col2 = so3.apply(Ru, ev)
col3 = so3.apply(Ru, so3.apply(Rv, ew))
#col1 = [0,0,1]
#col2 = [c0 -s0 0] [0] = [-s0]
# [s0 c0 0]*[1] [c0 ]
# [0 0 1] [0] [0 ]
#col3 = Ru*[c1 0 s1] [1] = Ru*[c1 ] = [c1c0]
# [0 1 0 ]*[0] [0 ] [c1s0]
# [-s1 0 c1] [0] [-s1] [-s1 ]
#A = [ 0 -s0 c1c0]
# [ 0 c0 c1s0]
# [ 1 0 -s1 ]
return zip(col1, col2, col3)
def solve_placement(self):
"""Implemented for you: come up with a collision-free target placement"""
obmin,obmax = self.objbb
ozmin = obmin[2]
min_dims = min(obmax[0]-obmin[0],obmax[1]-obmin[1])
center = [0.5*(obmax[0]+obmin[0]),0.5*(obmax[1]-obmin[1])]
xmin,ymin,zmin = self.goal_bounds[0]
xmax,ymax,zmax = self.goal_bounds[1]
center_sampling_range = [(xmin+min_dims/2,xmax-min_dims/2),(ymin+min_dims/2,ymax-min_dims/2)]
Tobj_feasible = []
for iters in range(20):
crand = [random.uniform(b[0],b[1]) for b in center_sampling_range]
Robj = so3.rotation((0,0,1),random.uniform(0,math.pi*2))
tobj = vectorops.add(so3.apply(Robj,[-center[0],-center[1],0]),[crand[0],crand[1],zmin-ozmin+0.002])
self.object.setTransform(Robj,tobj)
feasible = True
for i in range(self.world.numTerrains()):
if self.object.geometry().collides(self.world.terrain(i).geometry()):
feasible=False
break
if not feasible:
bmin,bmax = self.object.geometry().getBBTight()
if bmin[0] < xmin:
tobj[0] += xmin-bmin[0]
if bmax[0] > xmax:
tobj[0] -= bmin[0]-xmax
if bmin[1] < ymin:
tobj[1] += ymin-bmin[1]
if bmax[1] > ymax:
tobj[1] -= bmin[1]-ymax
self.object.setTransform(Robj,tobj)
feasible = True
for i in range(self.world.numTerrains()):
if self.object.geometry().collides(self.world.terrain(i).geometry()):
feasible=False
break
if not feasible:
continue
for i in range(self.world.numRigidObjects()):
if i == self.object.index: continue
if self.object.geometry().collides(self.world.rigidObject(i).geometry()):
#raise it up a bit
bmin,bmax = self.world.rigidObject(i).geometry().getBB()
tobj[2] = bmax[2]-ozmin+0.002
self.object.setTransform(Robj,tobj)
Tobj_feasible.append((Robj,tobj))
print("Found",len(Tobj_feasible),"valid object placements")
return Tobj_feasible
def xyz_rpy(xyz=None, rpy=None):
xyz = xyz or [0, 0, 0]
rpy = rpy or [0, 0, 0]
Rall = None
for (i, angle) in enumerate(rpy):
axis = [0, 0, 0]
axis[i] = 1
R = so3.rotation(axis, angle)
if Rall:
Rall = so3.mul(Rall, R)
else:
Rall = R
return klampt2numpy((Rall, xyz))
def main():
w = WorldModel()
w.readFile("../data/robots/kinova_gen3_7dof.urdf")
robot = w.robot(0)
#put a "pen" geometry on the end effector
gpen = Geometry3D()
res = gpen.loadFile("../data/objects/cylinder.off")
assert res
gpen.scale(0.01,0.01,0.15)
gpen.rotate(so3.rotation((0,1,0),math.pi/2))
robot.link(7).geometry().setCurrentTransform(*se3.identity())
gpen.transform(*robot.link(8).getParentTransform())
robot.link(7).geometry().set(merge_geometry.merge_triangle_meshes(gpen,robot.link(7).geometry()))
robot.setConfig(robot.getConfig())
return run_simulation(w)
def set_pos_on_palm(self, link_idx, pos, tilted_angle):
'''
change the position of link mounted on the palm to x, y
:param pos: tuple value (r, theta) on palm coordinate
:param link_idx: indicate link
:return:
'''
if np.abs(tilted_angle) > 30:
return
print('pass')
x = pos[0] * np.cos(pos[1] / 180 * np.pi)
y = pos[0] * np.sin(pos[1] / 180 * np.pi)
p_T = [x, y, 0]
angle = np.arctan2(x, y)
R_1 = so3.rotation([1, 0, 0], angle + math.radians(tilted_angle))
R_2 = (0, 0, -1, 0, -1, 0, -1, 0, 0)
p_R = so3.mul(R_2, R_1)
self.robot.link(link_idx).setParentTransform(p_R, p_T)
def find_target(self): self.current_target=self.waiting_list[0] best_p=self.sim.world.rigidObject(self.current_target).getTransform()[1] self.current_target_pos=best_p for i in self.waiting_list: p=self.sim.world.rigidObject(i).getTransform()[1] # print i,p[2],vectorops.distance([0,0],[p[0],p[1]]) if i not in self.failedpickup: if p[2]>best_p[2]+0.05: self.current_target=i self.current_target_pos=p # print 'higher is easier!' best_p=p elif p[2]>best_p[2]-0.04 and vectorops.distance([0,0],[p[0],p[1]])<vectorops.distance([0,0],[best_p[0],best_p[1]]): self.current_target=i self.current_target_pos=p best_p=p elif self.current_target in self.failedpickup: self.current_target=i self.current_target_pos=p best_p=p d_y=-0.02 face_down=face_down_forward self.tooclose = False; if best_p[1]>0.15: d_y=-0.02 face_down=face_down_backward self.tooclose = True; print 'too close to the wall!!' elif best_p[1]<-0.15: d_y=0.02 print best_p self.tooclose = True; print 'too close to the wall!!' #here is hardcoding the best relative position for grasp the ball target=(face_down,vectorops.add(best_p,[0,d_y,0.14])) if self.tooclose: if best_p[0]<0: self.boxside=boxleft aready_pos=start_pos_left else: self.boxside=boxright aready_pos=start_pos_right aready_pos[1][0]=best_p[0] balllocation = best_p handballdiff = vectorops.sub(aready_pos[1],best_p) axis = vectorops.unit(vectorops.cross(handballdiff,aready_pos[1])) axis = (1,0,0) if best_p[1]>0: axis=(-1,0,0) angleforaxis = -1*math.acos(vectorops.dot(aready_pos[1],handballdiff)/vectorops.norm(aready_pos[1])/vectorops.norm(handballdiff)) angleforaxis = so2.normalize(angleforaxis) #if angleforaxis>math.pi: # angleforaxis=angleforaxis-2*math.pi adjR = so3.rotation(axis, angleforaxis) print balllocation print vectorops.norm(aready_pos[1]),vectorops.norm(handballdiff),angleforaxis target=(adjR,vectorops.add(best_p,vectorops.div(vectorops.unit(handballdiff),5))) # print self.current_target # print 'find target at:',self.current_target_pos return target
return rgb,depth
world = klampt.WorldModel()
world.readFile("../../data/simulation_test_worlds/sensortest.xml")
#world.readFile("../../data/tx90scenario0.xml")
robot = world.robot(0)
vis.add("world",world)
sim = klampt.Simulator(world)
sensor = sim.controller(0).sensor("rgbd_camera")
print("sensor.getSetting('link'):",sensor.getSetting("link"))
print("sensor.getSetting('Tsensor'):",sensor.getSetting("Tsensor"))
input("Press enter to continue...")
#T = (so3.sample(),[0,0,1.0])
T = (so3.mul(so3.rotation([1,0,0],math.radians(-10)),[1,0,0, 0,0,-1, 0,1,0]),[0,-2.0,0.5])
sensing.set_sensor_xform(sensor,T,link=-1)
vis.add("sensor",sensor)
read_local = True
#Note: GLEW sensor simulation only runs if it occurs in the visualization thread (e.g., the idle loop)
class SensorTestWorld (GLPluginInterface):
def __init__(self):
GLPluginInterface.__init__(self)
robot.randomizeConfig()
#sensor.kinematicSimulate(world,0.01)
sim.controller(0).setPIDCommand(robot.getConfig(),[0.0]*7)
self.rgb=None
def deviceToWidgetTransform(R, t):
"""Given a device transform, map to the widget transform world coordinates."""
Tview = so3.rotation((0, 0, 1), math.pi), [0, 0, 0]
return se3.mul(Tview, deviceToViewTransform(R, t))
world = klampt.WorldModel()
world.readFile("../../data/simulation_test_worlds/sensortest.xml")
#world.readFile("../../data/tx90scenario0.xml")
robot = world.robot(0)
vis.add("world", world)
sim = klampt.Simulator(world)
sensor = sim.controller(0).sensor("rgbd_camera")
print("sensor.getSetting('link'):", sensor.getSetting("link"))
print("sensor.getSetting('Tsensor'):", sensor.getSetting("Tsensor"))
input("Press enter to continue...")
#T = (so3.sample(),[0,0,1.0])
T = (so3.mul(so3.rotation([1, 0, 0], math.radians(-10)),
[1, 0, 0, 0, 0, -1, 0, 1, 0]), [0, -2.0, 0.5])
sensing.set_sensor_xform(sensor, T, link=-1)
vis.add("sensor", sensor)
read_local = True
#Note: GLEW sensor simulation only runs if it occurs in the visualization thread (e.g., the idle loop)
class SensorTestWorld(GLPluginInterface):
def __init__(self):
GLPluginInterface.__init__(self)
robot.randomizeConfig()
#sensor.kinematicSimulate(world,0.01)
sim.controller(0).setPIDCommand(robot.getConfig(), [0.0] * 7)
def widgetToDeviceTransform(R, t):
"""Given a widget transform in the world coordinates, map to a device transform """
Tview = so3.rotation((0, 0, 1), math.pi), [0, 0, 0]
Rview, tview = se3.mul(se3.inv(Tview), (R, t))
return viewToDeviceTransform(Rview, tview)
w = WorldModel()
w.readFile("../data/robots/kinova_gen3_7dof.urdf")
w.readFile("../data/terrains/plane.env")
robot = w.robot(0)
obstacles = [w.terrain(0)]
ee_link = 'EndEffector_Link'
ee_local_pos = (0.15,0,0)
ee_local_axis = (1,0,0)
#put a "pen" geometry on the end effector
gpen = Geometry3D()
res = gpen.loadFile("../data/objects/cylinder.off")
assert res
gpen.scale(0.01,0.01,0.15)
gpen.rotate(so3.rotation((0,1,0),math.pi/2))
robot.link(7).geometry().setCurrentTransform(*se3.identity())
gpen.transform(*robot.link(8).getParentTransform())
robot.link(7).geometry().set(merge_geometry.merge_triangle_meshes(gpen,robot.link(7).geometry()))
robot.setConfig(robot.getConfig())
def show_workspace(grid):
vis.add("world",w)
res = numpy_convert.from_numpy((lower_corner,upper_corner,grid),'VolumeGrid')
g_workspace = Geometry3D(res)
g_surface = g_workspace.convert('TriangleMesh',0.5)
if g_surface.numElements() != 0:
vis.add("reachable_boundary",g_surface,color=(1,1,0,0.5))
else:
print("Nothing reachable?")
vis.add("cam", cam)
vp = vis.getViewport()
vp.camera.rot[1] -= 0.5
vis.setViewport(vp)
default = vis.getViewport().getTransform()
print('x:', so3.apply(default[0], [1, 0, 0]))
print('y:', so3.apply(default[0], [0, 1, 0]))
print('z:', so3.apply(default[0], [0, 0, 1]))
print('offset:', default[1])
circle_points = []
npts = 50
times = []
for i in range(npts + 1):
angle = math.radians(360 * i / npts)
circle_points.append(
se3.mul((so3.rotation([0, 0, 1], angle), [0, 0, 0]), default))
times.append(i * 20 / npts)
circle_traj = SE3Trajectory(times, circle_points)
circle_traj.milestones[-1] = circle_traj.milestones[0]
circle_smooth_traj = SE3HermiteTrajectory()
circle_smooth_traj.makeSpline(circle_traj, loop=True)
R0 = so3.identity()
R1 = so3.rotation([0, 0, 1], math.pi / 2)
dR0 = [0.0] * 9
#TODO: for some reason the interpolation doesn't work very well...
#vis.add("Camera smooth traj",circle_smooth_traj.discretize_se3(0.1))
#for m in circle_smooth_traj.milestones:
# T = m[:12]
# vT = m[12:]
# print("Orientation velocity",vT[:9])
TODO: the current implementation doesn't work properly. Why? """
m1 = so3.moment(R1)
m2 = so3.moment(R2)
mu = interpolate_linear(m1, m2, u)
angle = vectorops.norm(mu)
axis = vectorops.div(mu, angle)
return so3.rotation(axis, angle)
def interpolate_transform(T1, T2, u):
"""Interpolate linearly between the two transformations T1 and T2."""
return (interpolate_rotation(T1[0], T2[0],
u), interpolate_linear(T1[1], T2[1], u))
Ra = so3.rotation((0, 1, 0), math.pi * -0.9)
Rb = so3.rotation((0, 1, 0), math.pi * 0.9)
print("Angle between")
print(so3.matrix(Ra))
print("and")
print(so3.matrix(Rb))
print("is", so3.distance(Ra, Rb))
Ta = [Ra, [-1, 0, 1]]
Tb = [Rb, [1, 0, 1]]
if __name__ == "__main__":
vis.add("world", se3.identity())
vis.add("start", Ta)
vis.add("end", Tb)
vis.add("interpolated", Ta)
vis.edit("start")
