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 opt_error_fun(est_input, *args):
"""
:param est_input: {numpy.array} -- the initial guess of the transformation of Toct and Tneedle
:param args: {tuple} -- the set of tranfermation of Tlink and ICP transformation matrix
:return: error: {float} -- the error between the true transformation and estimated transformation
"""
Roct = so3.from_rpy(est_input[0:3])
toct = est_input[3:6]
Rn = so3.from_rpy(est_input[6:9])
tn = est_input[9:12]
Tlink_set = args[0]
Tneedle2oct_icp = args[1]
fun_list = np.array([])
for i in range(len(Tlink_set)):
fun_list = np.append(
fun_list,
np.multiply(
so3.error(so3.inv(Tneedle2oct_icp[i][0]),
so3.mul(so3.mul(so3.inv(Roct), Tlink_set[i][0]),
Rn)), 1))
fun_list = np.append(
fun_list,
np.multiply(
vectorops.sub(
vectorops.sub([0., 0., 0.],
so3.apply(so3.inv(Tneedle2oct_icp[i][0]),
Tneedle2oct_icp[i][1])),
so3.apply(
so3.inv(Roct),
vectorops.add(vectorops.sub(Tlink_set[i][1], toct),
so3.apply(Tlink_set[i][0], tn)))), 1000))
return fun_list
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 __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 get_desired_end_effector_rotation(self, leg_number, yaw_rads):
if leg_number == 1 or leg_number == 3:
r = [0, 0, -1, 0, -1, 0, -1, 0, 0]
else:
r = [0, 0, -1, 0, 1, 0, 1, 0, 0]
yaw_rotation_aa = ([0, 0, 1], yaw_rads)
yaw_rotation_R = so3.from_axis_angle(yaw_rotation_aa)
return so3.mul(yaw_rotation_R, r)
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 getDesiredCartesianPose(self,limb,device):
"""Returns a pair (R,t) of the desired EE pose if the limb should have
a cartesian pose message, or None if it should not.
- limb: either 'left' or 'right'
- device: an index of the haptic device
Implementation-wise, this reads from self.startTransforms and the deviceState
to determine the correct desired end effector transform. The delta
from devices[device]['deviceCurrentTransform'] to devices[device]['deviceInitialTransform']
is scaled, then offset by self.startTransforms[device]. (self.startTransforms is
the end effector transform when deviceInitialTransform is set)
"""
if limb=='left':
T = self.serviceThread.eeGetter_left.get()
else:
T = self.serviceThread.eeGetter_right.get()
if T is None:
T = se3.identity()
R,t=T
deviceState = self.haptic_client.deviceState
if deviceState == None: return T
dstate = deviceState[device]
if dstate.widgetInitialTransform[0] == None or self.startTransforms[device] == None: return T
Tcur = dstate.widgetCurrentTransform
T0 = dstate.widgetInitialTransform[0]
if T0 == None:
T0 = Tcur
#print "Button is down and mode is",dstate['mode']
#print dstate
assert T0 != None,"T0 is null"
assert Tcur != None,"Tcur is null"
relRot = so3.mul(Tcur[0],so3.inv(T0[0]))
relTrans = vectorops.sub(Tcur[1],T0[1])
#print "Rotation moment",so3.moment(relRot)
desRot = so3.mul(relRot,self.startTransforms[device][0])
desPos = vectorops.add(vectorops.mul(relTrans,hapticArmTranslationScaling),self.startTransforms[device][1])
#TEST: just use start rotation
#desRot = self.startTransforms[i][0]
return (desRot,desPos)
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 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 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 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 control_loop(self):
if self.sim.getTime()-self.last_end_time>0.5:
if self.trajectory:
if self.t<len(self.trajectory):
robot=self.sim.world.robot(0)
old_T=robot.link(ee_link).getTransform()
old_R,old_t=old_T
self.robotController.setLinear(self.trajectory[self.t][1]['robot'],self.trajectory[self.t][0]-(self.sim.getTime()-self.old_time))
robot.setConfig(self.trajectory[self.t][1]['robot'])
obj=self.sim.world.rigidObject(self.target)
#get a SimBody object
body = self.sim.body(obj)
# print self.trajectory[self.t][1]['vacuum']
if self.trajectory[self.t][1]['simulation']==1:
T=body.getTransform()
R,t=T
body.enableDynamics(False)
body.setVelocity((0,0,0),(0,0,0))
if self.flag==0:
self.flag=1
self.T=robot.link(ee_link).getLocalPosition(t)
# print 'here'
new_T=robot.link(ee_link).getTransform()
new_R,new_t=new_T
change_R=so3.mul(new_R,so3.inv(old_R))
R=so3.mul(change_R,R)
body.setTransform(R,robot.link(ee_link).getWorldPosition(self.T))
else:
body.enableDynamics(True)
self.flag=0
# print 'current config', robot.getConfig()
# print 'goal config',self.trajectory[self.t][1]['robot']
# if vectorops.distance(robot.getConfig(),self.trajectory[self.t][1]['robot'])<0.01:
# print 'get there'
if self.sim.getTime()-self.old_time>self.trajectory[self.t][0]:
self.t+=1
self.old_time=self.sim.getTime()
if self.t==len(self.trajectory):
self.last_end_time=self.sim.getTime()
else:
self.score+=1
self.in_box.append(self.target)
if self.task=='pick':
self.target+=1
if self.target>=self.total_object:
self.task='stow'
if self.task=='stow':
self.target-=1
print "one pick task is done!"
self.t=0
self.trajectory=[]
print 'score:',self.score
time.sleep(1)
if self.score==2*self.total_object:
print "all items are picked up!"
print self.sim.world.robot(0).getConfig()
exit()
else:
if self.score<self.total_object:
target_item={}
target_box={}
if max(self.sim.world.rigidObject(self.target).getVelocity()[0])>0.01:
print 'turning!'
return
target_item["position"]=self.sim.world.rigidObject(self.target).getTransform()[1]
self.place_position.append(self.sim.world.rigidObject(self.target).getTransform()[1])
target_item["vacuum_offset"]=[0,0,0.1]
target_item["bbox"]=self.sim.world.rigidObject(self.target).geometry().getBB()
target_item['drop offset']=[0,0,0.2]
target_box["box_limit"]=[[-0.5,0.2,0.1],[-0.35,0.45,0.4]]
# target_box["drop position"]=[-0.4,0.3,0.2]
target_box["drop position"]=[]
target_box['position']=[0.2,0.8,0.15]
old_config=self.sim.world.robot(0).getConfig()
self.trajectory=planner.pick_up(self.sim.world,target_item,target_box,self.target)
if self.trajectory==False:
self.score+=1
self.target+=1
if self.score==self.total_object:
self.task='stow'
self.target-=1
else:
self.old_time=self.sim.getTime()
self.time_count=0
print "validing trajectory..."
max_change_joint=0
max_joint_speed=0
for i in range(len(self.trajectory)):
new_config=self.trajectory[i][1]['robot']
d_config=max(max(vectorops.sub(new_config,old_config)),-min(vectorops.sub(new_config,old_config)))
speed_config=d_config/self.trajectory[i][0]
if d_config>max_change_joint:
max_change_joint=d_config
if speed_config>max_joint_speed:
max_joint_speed=speed_config
old_config=new_config
print 'max joint change is :', max_change_joint/3.14159*180
print 'max joint speed is:', max_joint_speed/3.14159*180
else:
target_item={}
target_box={}
# print self.sim.world.rigidObject(self.target).getVelocity()[0]
if max(self.sim.world.rigidObject(self.target).getVelocity()[0])>0.01:
print 'turning!'
return
target_item["position"]=self.sim.world.rigidObject(self.target).getTransform()[1]
target_item["vacuum_offset"]=[0,0,0.1]
target_item['drop offset']=[0,0,0.15]
target_item["bbox"]=self.sim.world.rigidObject(self.target).geometry().getBB()
# target_box["drop position"]=self.place_position[self.target]
# target_box['position']=self.place_position[self.target]
target_box["box_limit"]=[[-0.2,0.3,0.1],[0.6,0.5,0.4]]
# print self.place_position[self.target]
target_box["drop position"]=[]
target_box['position']=[]
old_config=self.sim.world.robot(0).getConfig()
self.trajectory=planner.stow(self.sim.world,target_item,target_box,self.target)
if self.trajectory==False:
self.score+=1
self.target-=1
if self.score==2*self.total_object:
print 'done!'
exit()
else:
self.old_time=self.sim.getTime()
self.time_count=0
print "validing trajectory..."
max_change_joint=0
max_joint_speed=0
for i in range(len(self.trajectory)):
new_config=self.trajectory[i][1]['robot']
d_config=max(max(vectorops.sub(new_config,old_config)),-min(vectorops.sub(new_config,old_config)))
speed_config=d_config/self.trajectory[i][0]
if d_config>max_change_joint:
max_change_joint=d_config
if speed_config>max_joint_speed:
max_joint_speed=speed_config
old_config=new_config
print 'max joint change is :', max_change_joint/3.14159*180
print 'max joint speed is:', max_joint_speed/3.14159*180
def onMessage(self, msg):
#print "Getting haptic message"
#print msg
self.numMessages += 1
if msg['type'] != 'MultiHapticState':
print "Strange message type", msg['type']
return
if len(self.deviceState) == 0:
print "Adding", len(msg['devices']) - len(
self.deviceState), "haptic devices"
while len(self.deviceState) < len(msg['devices']):
self.deviceState.append(HapticDeviceState())
if len(msg['devices']) != len(self.deviceState):
print "Incorrect number of devices in message:", len(
msg['devices'])
return
#read button presses
for dstate in self.deviceState:
dstate.buttonPressed[0] = dstate.buttonPressed[1] = False
dstate.buttonReleased[0] = dstate.buttonReleased[1] = False
if 'events' in msg:
for e in msg['events']:
#print e['type']
if e['type'] == 'b1_down':
dstate = self.deviceState[e['device']]
dstate.buttonPressed[0] = True
elif e['type'] == 'b2_down':
dstate = self.deviceState[e['device']]
dstate.buttonPressed[1] = True
elif e['type'] == 'b1_up':
dstate = self.deviceState[e['device']]
dstate.buttonReleased[0] = True
elif e['type'] == 'b2_up':
dstate = self.deviceState[e['device']]
dstate.buttonReleased[1] = True
for i in range(len(self.deviceState)):
dmsg = msg['devices'][i]
dstate = self.deviceState[i]
if dstate.time == dmsg['time']:
#no update...
continue
dstate.newupdate = True
# ===== read from msg =====
dstate.position = dmsg['position']
dstate.rotationMoment = dmsg['rotationMoment']
dstate.velocity = dmsg['velocity']
dstate.angularVelocity = dmsg['angularVelocity']
dstate.jointAngles = dmsg['jointAngles']
#dstate.gimbalAngle = dmsg['gimbalAngle']
oldTime = dstate.time
dstate.time = dmsg['time']
#print "position",dmsg['position']
#print "rotation moment",dmsg['rotationMoment']
#print "angular velocity",dmsg['angularVelocity']
dstate.widgetCurrentTransform = deviceToWidgetTransform(
so3.from_moment(dmsg['rotationMoment']), dmsg['position'])
if dmsg['button1'] == 1:
oldButtonDown = dstate.buttonDown[0]
dstate.buttonDown[0] = True
if not oldButtonDown:
dstate.widgetInitialTransform[
0] = dstate.widgetPreviousTransform
continue
else:
dstate.buttonDown[0] = False
if dmsg['button2'] == 1:
oldButtonDown = dstate.buttonDown[1]
dstate.buttonDown[1] = True
if not oldButtonDown:
dstate.widgetInitialTransform[
1] = dstate.widgetPreviousTransform
continue
else:
dstate.buttonDown[1] = False
newTime = dstate.time
if newTime != oldTime and dstate.widgetPreviousTransform is not None:
# print "previous position = ", dstate.widgetPreviousTransform[1]
# print "current position = ", dstate.widgetCurrentTransform[1]
timeInterval = newTime - oldTime
#print "========================"
#print "time = ", timeInterval
delta_Pos = vectorops.sub(dstate.widgetCurrentTransform[1],
dstate.widgetPreviousTransform[1])
vel = vectorops.div(delta_Pos, timeInterval)
delta_Moment = so3.moment(
so3.mul(dstate.widgetCurrentTransform[0],
so3.inv(dstate.widgetPreviousTransform[0])))
angvel = vectorops.div(delta_Moment, timeInterval)
# print "vel = [%2.4f %2.4f %2.4f]" % (vel[0], vel[1], vel[2])
# print "angvel = [%2.4f %2.4f %2.4f]" % (angvel[0], angvel[1], angvel[2])
dstate.linearVel = list(vel)
dstate.angularVel = list(angvel)
#end of loop, store previous transform
dstate.widgetPreviousTransform = dstate.widgetCurrentTransform
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 deviceToViewTransform(R, t):
Ruser = so3.mul(so3.inv(handleToUser[0]), R)
R = so3.mul(userToWorld[0], so3.mul(Ruser, worldToUser[0]))
return (R, so3.apply([0, 1, 0, 0, 0, 1, 1, 0, 0], t))
def drive(self, qcur, angVel, vel, dt):
"""Drives the robot by an incremental time step to reach the desired
Cartesian (angular/linear) velocities of the links previously specified
in start().
Args:
qcur (list of floats): The robot's current configuration.
angVel (3-vector or list of 3-vectors): the angular velocity of
each driven link. Set angVel to None to turn off orientation
control of every constraint. angVel[i] to None to turn off
orientation control of constraint i.
vel (3-vector or list of 3-vectors): the linear velocity of each
driven link. Set vel to None to turn off position control of
every constraint. Set vel[i] to None to turn off position
control of constraint i.
dt (float): the time step.
Returns:
tuple: A pair ``(progress,qnext)``. ``progress`` gives a value
in the range [0,1] indicating indicating how far along the nominal
drive amount the solver was able to achieve. If the result is < 0,
this indicates that the solver failed to make further progress.
``qnext`` is the resulting configuration that should be sent to the
robot's controller.
For longer moves, you should pass qnext back to this function as qcur.
"""
if angVel is None:
#turn off orientation control
if vel is None:
#nothing specified
return (1.0, qcur)
angVel = [None] * len(self.links)
else:
assert len(angVel) > 0
if not hasattr(angVel[0], '__iter__'):
angVel = [angVel]
if vel is None:
#turn off position control
vel = [None] * len(self.links)
else:
assert len(vel) > 0
if not hasattr(vel[0], '__iter__'):
vel = [vel]
if len(qcur) != self.robot.numLinks():
raise ValueError(
"Invalid size of current configuration, %d != %d" %
(len(qcur), self.robot.numLinks()))
if len(angVel) != len(self.links):
raise ValueError(
"Invalid # of angular velocities specified, %d != %d" %
(len(angVel), len(self.links)))
if len(vel) != len(self.links):
raise ValueError(
"Invalid # of translational velocities specified, %d != %d" %
(len(vel), len(self.links)))
anyNonzero = False
zeroVec = [0, 0, 0]
for (w, v) in zip(angVel, vel):
if w is not None and list(w) != zeroVec:
anyNonzero = True
break
if v is not None and list(v) != zeroVec:
anyNonzero = True
break
if not anyNonzero:
return (1.0, qcur)
qout = [v for v in qcur]
#update drive transforms
self.robot.setConfig(qcur)
robotLinks = [self.robot.link(l) for l in self.links]
#limit the joint movement by joint limits and velocity
tempqmin, tempqmax = self.robot.getJointLimits()
if self.qmin is not None:
tempqmin = self.qmin
if self.qmax is not None:
tempqmax = self.qmax
vmax = self.robot.getVelocityLimits()
vmin = vectorops.mul(vmax, -1)
if self.vmin is not None:
vmin = self.vmin
if self.vmax is not None:
vmax = self.vmax
for i in range(self.robot.numLinks()):
tempqmin[i] = max(tempqmin[i], qcur[i] + dt * vmin[i])
tempqmax[i] = min(tempqmax[i], qcur[i] + dt * vmax[i])
#Setup the IK solver parameters
self.solver.setJointLimits(tempqmin, tempqmax)
tempGoals = [ik.IKObjective(g) for g in self.ikGoals]
for i in range(len(self.links)):
if math.isfinite(self.positionTolerance) and math.isfinite(
self.rotationTolerance):
tempGoals[i].rotationScale = self.positionTolerance / (
self.positionTolerance + self.rotationTolerance)
tempGoals[i].positionScale = self.rotationTolerance / (
self.positionTolerance + self.rotationTolerance)
elif not math.isfinite(
self.positionTolerance) and not math.isfinite(
self.rotationTolerance):
pass
else:
tempGoals[i].rotationScale = min(
self.positionTolerance,
self.rotationTolerance) / self.rotationTolerance
tempGoals[i].positionScale = min(
self.positionTolerance,
self.rotationTolerance) / self.positionTolerance
tolerance = min(
1e-6,
min(self.positionTolerance, self.rotationTolerance) /
(math.sqrt(3.0) * len(self.links)))
self.solver.setTolerance(tolerance)
#want to solve max_t s.t. there exists q satisfying T_i(q) = TPath_i(t)
#where TPath_i(t) = Advance(Tdrive_i,Screw_i*t)
if self.driveSpeedAdjustment == 0:
self.driveSpeedAdjustment = 0.1
anyFailures = False
while self.driveSpeedAdjustment > 0:
#advance the desired cartesian goals
#set up IK parameters: active dofs, IKGoal
amount = dt * self.driveSpeedAdjustment
desiredTransforms = [[None, None] for i in range(len(self.links))]
for i in range(len(self.links)):
if vel[i] is not None:
desiredTransforms[i][1] = vectorops.madd(
self.driveTransforms[i][1], vel[i], amount)
tempGoals[i].setFixedPosConstraint(
self.endEffectorOffsets[i], desiredTransforms[i][1])
else:
tempGoals[i].setFreePosition()
if angVel[i] is not None:
increment = so3.from_moment(
vectorops.mul(angVel[i], amount))
desiredTransforms[i][0] = so3.mul(
increment, self.driveTransforms[i][0])
tempGoals[i].setFixedRotConstraint(desiredTransforms[i][0])
else:
tempGoals[i].setFreeRotConstraint()
self.solver.set(i, tempGoals[i])
err0 = self.solver.getResidual()
quality0 = vectorops.norm(err0)
res = self.solver.solve()
q = self.robot.getConfig()
activeDofs = self.solver.getActiveDofs()
for k in activeDofs:
if q[k] < tempqmin[k] or q[k] > tempqmax[k]:
#the IK solver normalizer doesn't care about absolute
#values for joints that wrap around 2pi
if tempqmin[k] <= q[k] + 2 * math.pi and q[
k] + 2 * math.pi <= tempqmax[k]:
q[k] += 2 * math.pi
elif tempqmin[k] <= q[k] - 2 * math.pi and q[
k] - 2 * math.pi <= tempqmax[k]:
q[k] -= 2 * math.pi
else:
print(
"CartesianDriveSolver: Warning, result from IK solve is out of bounds: index",
k, ",", tempqmin[k], "<=", q[k], "<=", tempqmax[k])
q[k] = max(min(q[k], tempqmax[k]), tempqmin[k])
self.robot.setConfig(q)
#now evaluate quality of the solve
errAfter = self.solver.getResidual()
qualityAfter = vectorops.norm(errAfter)
if qualityAfter > quality0:
#print("CartesianDriveSolver: Solve failed: original configuration was better:",quality0,"vs",qualityAfter)
#print(" solver result was",res,"increment",amount)
res = False
elif qualityAfter < quality0 - 1e-8:
res = True
if res:
#success!
for k in activeDofs:
qout[k] = q[k]
assert tempqmin[k] <= q[k] and q[k] <= tempqmax[k]
#now advance the driven transforms
#figure out how much to drive along screw
numerator = 0 # this will get sum of distance * screws
denominator = 0 # this will get sum of |screw|^2 for all screws
#result will be numerator / denominator
achievedTransforms = [
(l.getTransform()[0], l.getWorldPosition(ofs))
for l, ofs in zip(robotLinks, self.endEffectorOffsets)
]
#TODO: get transforms relative to baseLink
for i in range(len(self.links)):
if vel[i] is not None:
trel = vectorops.sub(achievedTransforms[i][1],
self.driveTransforms[i][1])
vellen = vectorops.norm(vel[i])
axis = vectorops.div(vel[i], max(vellen, 1e-8))
ut = vellen
tdistance = vectorops.dot(trel, axis)
tdistance = min(max(tdistance, 0.0), dt * vellen)
numerator += ut * tdistance
denominator += ut**2
if angVel[i] is not None:
Rrel = so3.mul(achievedTransforms[i][0],
so3.inv(self.driveTransforms[i][0]))
vellen = vectorops.norm(angVel[i])
angVelRel = so3.apply(
so3.inv(self.driveTransforms[i][0]), angVel[i])
rotaxis = vectorops.div(angVelRel, max(vellen, 1e-8))
Rdistance = axis_rotation_magnitude(Rrel, rotaxis)
Rdistance = min(max(Rdistance, 0.0), dt * vellen)
uR = vellen
numerator += uR * Rdistance
denominator += uR**2
distance = numerator / max(denominator, 1e-8)
#computed error-minimizing distance along screw motion
for i in range(len(self.links)):
if vel[i] is not None:
self.driveTransforms[i][1] = vectorops.madd(
self.driveTransforms[i][1], vel[i], distance)
else:
self.driveTransforms[i][1] = achievedTransforms[i][1]
if angVel[i] is not None:
Rincrement = so3.from_moment(
vectorops.mul(angVel[i], distance))
self.driveTransforms[i][0] = normalize_rotation(
so3.mul(Rincrement, self.driveTransforms[i][0]))
else:
self.driveTransforms[i][0] = achievedTransforms[i][0]
if math.ceil(distance / dt * 10) < math.floor(
self.driveSpeedAdjustment * 10):
self.driveSpeedAdjustment -= 0.1
self.driveSpeedAdjustment = max(self.driveSpeedAdjustment,
0.0)
elif not anyFailures:
#increase drive velocity
if self.driveSpeedAdjustment < 1.0:
self.driveSpeedAdjustment += 0.1
self.driveSpeedAdjustment = min(
1.0, self.driveSpeedAdjustment)
return (distance / dt, qout)
else:
#IK failed: try again with a slower speed adjustment
anyFailures = True
self.driveSpeedAdjustment -= 0.1
if self.driveSpeedAdjustment <= 1e-8:
self.driveSpeedAdjustment = 0.0
break
#no progress
return (-1, qcur)
def viewToDeviceTransform(R, t):
Ruser = so3.mul(worldToUser[0], so3.mul(R, userToWorld[0]))
R = so3.mul(handleToUser[0], Ruser)
return (R, so3.apply(so3.inv([0, 1, 0, 0, 0, 1, 1, 0, 0]), t))
import math
#two finger configurations
c_hand = 0.28
o_hand = 0.6
power_hand = 0
precision_hand = 0.75
power_close_hand = [c_hand, c_hand, c_hand, power_hand]
power_open_hand = [o_hand - 0.02, o_hand - 0.02, o_hand - 0.02, power_hand]
precision_close_hand = [c_hand, c_hand, c_hand, precision_hand]
precision_open_hand = [o_hand, o_hand, o_hand, precision_hand]
#hand orientation
move_speed = 0.5
face_down = [0, -1, 0, 1, 0, 0, 0, 0, 1]
face_down_rot = [-1, 0, 0, 0, -1, 0, 0, 0, 1]
# face_toward_shelf=[1,0,0,0,0,1,0,-1,0]
face_toward_shelf = so3.mul(
so3.from_axis_angle(([1, 0, 0], math.pi / 180 * 70)), face_down)
idle_pos = (face_down, [0, 0.5, 0.77])
start_pos_face_down = (face_down, [0, 0.4, 0.77])
start_pos_face_down_rot = (face_down_rot, [0, 0.4, 0.77])
start_pos_face_toward_shelf = (face_toward_shelf, [0, 0.4, 0.77])
drop_pos_face_down = (face_down, [0, -0.1, 0.77])
drop_pos_face_down_rot = (face_down_rot, [0, -0.1, 0.77])
drop_pos_face_toward_shelf = (face_toward_shelf, [0, -0.2, 0.77])
sweep_pos = so3.mul(so3.from_axis_angle(([1, 0, 0], math.pi / 180 * 70)),
face_down_rot)
start_pos_sweep = (sweep_pos, [0, 0.55, 0.77])
drop_pos_sweep = (sweep_pos, [0, -0.1, 0.77])
# sweep_pos_1=(face_down,[-0.03,0.85,0.7])
# sweep_pos_2=(face_down,[-0.03,0.85,0.59])
def rotationDeltaFromButtonPress(self, button=0):
"""Returns the difference between the current rotation and the button press rotation,
relative to the frame of the button press rotation"""
return so3.mul(self.widgetCurrentTransform[0],
so3.inv(self.widgetInitialTransform[button][0]))
def control_loop(self):
#Calculate the desired velocity for each robot by adding up all
#commands
rvels = [[0] * self.world.robot(r).numLinks()
for r in range(self.world.numRobots())]
robot = self.world.robot(0)
ee_link = 6
ee_local_p1 = [-0.015, -0.02, 0.27]
q_curr = robot.getConfig()
q_curr[3] = -q_curr[3]
q_curr[5] = -q_curr[5]
q_output = vectorops.mul(q_curr[1:], angle_to_degree)
milestone = (0.1, {
'robot': q_output,
'gripper': [0, 0, 0],
'vaccum': [0]
})
self.output.append(milestone)
if self.start_flag == 0:
curr_position = robot.link(ee_link).getWorldPosition(ee_local)
curr_orientation, p = robot.link(ee_link).getTransform()
self.start_T = [curr_orientation, curr_position]
self.start_flag = 1
local_p = []
if self.state == 'idle':
t = 0.5
if self.score == self.numberOfObjects:
print 'finished!'
f = open('test.json', 'w')
json.dump(self.output, f)
f.close()
exit()
else:
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
self.end_T = [[0, 0, -1, 0, 1, 0, 1, 0, 0], idle_position]
# self.idle_T=self.end_T
# self.sim.controller(0).setPIDCommand(self.idleq,[0]*7)
[local_p, world_p] = interpolate(self.start_T, self.end_T,
u, 1)
# constraints=ik.objective(robot.link(ee_link),local=local_p,world=world_p)
# traj1 = cartesian_trajectory.cartesian_interpolate_linear(robot,self.start_T,self.end_T,constraints,delta=1e-2,maximize=False)
# print 'traj1:',traj1[0]
else:
# print 'idle', robot.getConfig()
self.set_state('find_target')
elif self.state == 'find_target':
h = 0
for i in self.waiting_list:
if self.sim.world.rigidObject(i).getTransform()[1][2] > h:
self.target = i
h = self.sim.world.rigidObject(i).getTransform()[1][2]
print 'target is ', self.target
# self.target=0
# self.object_p=self.sim.world.rigidObject(self.target).getTransform()[1]
bb1, bb2 = self.sim.world.rigidObject(
self.target).geometry().getBB()
self.object_p = vectorops.div(vectorops.add(bb1, bb2), 2)
# print 'object xform', self.sim.world.rigidObject(self.target).getTransform()[1]
# print 'object_p',self.object_p
h = self.object_p[2] + box_vacuum_offset[2]
if self.object_p[1] > shelf_position[1]:
end_p = approach_p1
end_p[2] = h
else:
end_p = approach_p2
end_p[2] = h
d = vectorops.distance(end_p[:1], self.object_p[:1])
dx = self.object_p[0] - end_p[0]
dy = self.object_p[1] - end_p[1]
self.end_T = [[0, 0, -1, -dy / d, dx / d, 0, dx / d, dy / d, 0],
end_p]
print 'approach start position', end_p
self.retract_T = [[
0, 0, -1, -dy / d, dx / d, 0, dx / d, dy / d, 0
], end_p]
print 'retract_T', self.retract_T
self.set_state('preposition')
elif self.state == 'preposition':
t = 0.5
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
1)
else:
self.end_T[1] = vectorops.add(self.object_p, [0.10, 0, 0.12])
self.set_state('pregrasp')
elif self.state == 'pregrasp':
t = 0.5
# print 'pregrasp'
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
1)
else:
self.end_T[1][2] -= 0.03
self.set_state('grasp')
elif self.state == 'grasp':
# print 'grasp'
t = 0.5
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
1)
else:
self.end_T[1][2] += 0.03
self.set_state('prograsp')
elif self.state == 'prograsp':
# print 'grasp'
self.graspedObjects[self.target] = True
t = 0.5
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
1)
else:
self.end_T = self.retract_T
self.end_T[1][2] += 0.01
self.set_state('retract')
elif self.state == 'retract':
# print 'retract'
t = 0.5
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
1)
else:
self.set_state('go_to_tote')
elif self.state == 'go_to_tote':
x = robot.link(ee_link).getTransform()[1][0]
if x > (order_box_min[0] + order_box_max[0]) / 2:
q = robot.getConfig()
q[1] += 0.02
# self.sim.controller(0).setPIDCommand(q,[0]*7)
robotController.setLinear(q, 0.001)
# self.output.append(vectorops.mul(q[1:],angle_to_degree))
else:
bb1, bb2 = self.sim.world.rigidObject(
self.target).geometry().getBB()
bbx = bb2[0] - bb1[0]
bby = bb2[1] - bb1[1]
print 'BB:', bbx, bby
# self.end_T[1]=[order_box_min[0]+bbx/2-0.01,order_box_min[1]+bby/2+0.21-self.target*0.1,0.8]
self.end_T[1] = self.find_placement()
# if self.score>0:
# pass
self.end_T[0] = [0, 0, -1, -1, 0, 0, 0, 1, 0]
self.set_state('place')
elif self.state == 'place':
t = 0.5
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
0)
else:
self.end_T[1][2] -= 0.01
self.set_state('release')
elif self.state == 'release':
t = 0.5
if self.sim.getTime() - self.last_state_end < t:
u = (self.sim.getTime() - self.last_state_end) / t
[local_p, world_p] = interpolate(self.start_T, self.end_T, u,
0)
else:
self.graspedObjects[self.target] = False
self.check_target()
self.set_state('idle')
old_T = robot.link(ee_link).getTransform()
old_R, old_t = old_T
if local_p:
q_old = robot.getConfig()
goal = ik.objective(robot.link(ee_link),
local=local_p,
world=world_p)
# s=ik.solver(goal)
# s.setMaxIters(100)
# s.setTolerance(0.2)
# if s.solve():
# pass
# else:
# print "IK failed"
# while not s.solve():
# q_restart=[0,0,0,0,0,0,0]
# q=robot.getConfig()
# (qmin,qmax) = robot.getJointLimits()
# for i in range(7):
# q_restart[i]=random.uniform(-1,1)+q[i]
# q_restart[i] = min(qmax[i],max(q_restart[i],qmin[i]))
# print q_restart
# robot.setConfig(q_restart)
# if s.solve():
# print "IK solved"
# else:
# print "IK failed"
# s=ik.solve_global(goal)
s = ik.solve_nearby(goal,
maxDeviation=10,
feasibilityCheck=test_function)
q = robot.getConfig()
if not feasible(robot, q, q_old):
s = ik.solve_global(goal)
q = robot.getConfig()
robotController = self.sim.controller(0)
qdes = q
(qmin, qmax) = robot.getJointLimits()
for i in xrange(len(qdes)):
qdes[i] = min(qmax[i], max(qdes[i], qmin[i]))
# robotController.setPIDCommand(qdes,rvels[0])
robotController.setLinear(qdes, 0.001)
# self.output.append(vectorops.mul(qdes[1:],angle_to_degree))
obj = self.sim.world.rigidObject(self.target)
#get a SimBody object
body = self.sim.body(obj)
if self.graspedObjects[self.target]:
T = body.getTransform()
R, t = T
body.enableDynamics(False)
if self.flag == 0:
self.flag = 1
self.t = robot.link(ee_link).getLocalPosition(t)
# print 'here'
new_T = robot.link(ee_link).getTransform()
new_R, new_t = new_T
change_R = so3.mul(new_R, so3.inv(old_R))
R = so3.mul(change_R, R)
body.setTransform(R, robot.link(ee_link).getWorldPosition(self.t))
else:
body.enableDynamics(True)
self.flag = 0
return
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 broadcast_tf(broadcaster,
klampt_obj,
frameprefix="klampt",
root="world",
stamp=0.):
"""Broadcasts Klampt frames to the ROS tf module.
Args:
broadcaster (tf.TransformBroadcaster): the tf broadcaster
klampt_obj: the object to publish. Can be WorldModel, Simulator,
RobotModel, SimRobotController, anything with a getTransform
or getCurrentTransform method, or se3 element
frameprefix (str): the name of the base frame for this object
root (str): the name of the TF world frame.
Note:
If klampt_obj is a Simulator or SimRobotController, then its
corresponding model will be updated.
"""
from klampt import WorldModel, Simulator, RobotModel, SimRobotController, SimRobotSensor
if stamp == 'now':
stamp = rospy.Time.now()
elif isinstance(stamp, (int, float)):
stamp = rospy.Time(stamp)
world = None
if isinstance(klampt_obj, WorldModel):
world = klampt_obj
elif isinstance(klampt_obj, Simulator):
world = klampt_obj.model()
klampt_obj.updateModel()
if world is not None:
prefix = frameprefix
for i in xrange(world.numRigidObjects()):
T = world.rigidObject(i).getTransform()
q = so3.quaternion(T[0])
broadcaster.sendTransform(
T[1], (q[1], q[2], q[3], q[0]), stamp,
prefix + "/" + world.rigidObject(i).getName(), root)
for i in xrange(world.numRobots()):
robot = world.robot(i)
rprefix = prefix + "/" + robot.getName()
broadcast_tf(broadcaster, robot, rprefix, root)
return
robot = None
if isinstance(klampt_obj, RobotModel):
robot = klampt_obj
elif isinstance(klampt_obj, SimRobotController):
robot = klampt_obj.model()
robot.setConfig(klampt_obj.getSensedConfig())
if robot is not None:
rprefix = frameprefix
for j in xrange(robot.numLinks()):
p = robot.link(j).getParent()
if p < 0:
T = robot.link(j).getTransform()
q = so3.quaternion(T[0])
broadcaster.sendTransform(
T[1], (q[1], q[2], q[3], q[0]), stamp,
rprefix + "/" + robot.link(j).getName(), root)
else:
Tparent = se3.mul(se3.inv(robot.link(p).getTransform()),
robot.link(j).getTransform())
q = so3.quaternion(Tparent[0])
broadcaster.sendTransform(
Tparent[1], (q[1], q[2], q[3], q[0]), stamp,
rprefix + "/" + robot.link(j).getName(),
rprefix + "/" + robot.link(p).getName())
return
if isinstance(klampt_obj, SimRobotSensor):
from ..model import sensing
Tsensor_link = sensing.get_sensor_xform(klampt_obj)
link = int(klampt_obj.getSetting('link'))
if klampt_obj.type(
) == 'LaserRangeSensor': #the convention between Klampt and ROS is different
klampt_to_ros_lidar = so3.from_matrix([[0, 1, 0], [0, 0, 1],
[1, 0, 0]])
Tsensor_link = (so3.mul(Tsensor_link[0],
klampt_to_ros_lidar), Tsensor_link[1])
q = so3.quaternion(Tsensor_link[0])
robot = klampt_obj.robot()
broadcaster.sendTransform(
Tsensor_link[1], (q[1], q[2], q[3], q[0]), stamp,
frameprefix + "/" + robot.getName() + '/' + klampt_obj.name(),
frameprefix + "/" + robot.link(link).getName())
return
transform = None
if isinstance(klampt_obj, (list, tuple)):
#assume to be an SE3 element.
transform = klampt_obj
elif hasattr(klampt_obj, 'getTransform'):
transform = klampt_obj.getTransform()
elif hasattr(klampt_obj, 'getCurrentTransform'):
transform = klampt_obj.getCurrentTransform()
else:
raise ValueError("Invalid type given to broadcast_tf: ",
klampt_obj.__class__.__name__)
q = so3.quaternion(transform[0])
broadcaster.sendTransform(transform[1], (q[1], q[2], q[3], q[0]), stamp,
frameprefix, root)
return
