def calculate_workspace_free(robot,obstacles,end_effector,point_local):
"""Calculate the reachable workspace of the end effector point whose coordinates
are `point_local` on link `end_effector`. Ensure that the robot is collision
free with itself and with environment obstacles
"""
global lower_corner,upper_corner
resolution = (20,20,20)
cellsize = vectorops.div(vectorops.sub(upper_corner,lower_corner),resolution)
invcellsize = vectorops.div(resolution,vectorops.sub(upper_corner,lower_corner))
reachable = np.zeros(resolution)
#TODO: your code here
feasible = collision_free(robot,obstacles)
if feasible:
wp = end_effector.getWorldPosition(point_local)
index = [int(math.floor(v)) for v in vectorops.mul(vectorops.sub(wp,lower_corner),invcellsize)]
if all(i>=0 and i<r for (i,r) in zip(index,resolution)):
reachable[tuple(index)] = 1.0
# print(index)
num_samples = 100000
rand_positions = np.random.rand(num_samples, 3)
rand_positions[:,0] = rand_positions[:,0] * vectorops.sub(upper_corner, lower_corner)[0] + lower_corner[0]
rand_positions[:,1] = rand_positions[:,1] * vectorops.sub(upper_corner, lower_corner)[1] + lower_corner[1]
rand_positions[:,2] = rand_positions[:,2] * upper_corner[2]
for i in range(num_samples):
index = [int(math.floor(v)) for v in vectorops.mul(vectorops.sub(rand_positions[i],lower_corner),invcellsize)]
for obstacle in obstacles:
if obstacle.geometry().distance_point(rand_positions[i]).d <= 0:
reachable[tuple(index)] = 0.0
else:
reachable[tuple(index)] = 1.0
return reachable
def calculate_workspace_axis(robot, obstacles, end_effector, point_local,
axis_local, axis_world):
global lower_corner, upper_corner
resolution = (15, 15, 15)
cellsize = vectorops.div(vectorops.sub(upper_corner, lower_corner),
resolution)
invcellsize = vectorops.div(resolution,
vectorops.sub(upper_corner, lower_corner))
reachable = np.zeros(resolution)
#TODO: your code here
return reachable
def getCommandVelocity(self, q, dq, dt):
"""Gets the command velocity from the current state of the
robot.
"""
eP = self.getSensedError(q)
#vcmd = hP*eP + hD*eV + hI*eI
vP = gen_mul(self.hP, eP)
vcmd = vP
#D term
vcur = self.getSensedVelocity(q, dq, dt)
eD = None
if vcur != None:
eD = vectorops.sub(vcur, self.dxdes)
vD = gen_mul(self.hD, eD)
vcmd = vectorops.add(vcmd, vD)
#I term
if self.eI != None:
vI = gen_mul(self.hI, self.eI)
vcmd = vectorops.add(vcmd, vI)
#print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI
#do acceleration limiting
if vcur != None:
dvcmd = vectorops.div(vectorops.sub(vcmd, vcur), dt)
dvnorm = vectorops.norm(dvcmd)
if dvnorm > self.dvcmdmax:
vcmd = vectorops.madd(vcur, dvcmd, self.dvcmdmax / dvnorm * dt)
print self.name, "acceleration limited by factor", self.dvcmdmax / dvnorm * dt, "to", vcmd
#do velocity limiting
vnorm = vectorops.norm(vcmd)
if vnorm > self.vcmdmax:
vcmd = vectorops.mul(vcmd, self.vcmdmax / vnorm)
print self.name, "velocity limited by factor", self.vcmdmax / vnorm, "to", vcmd
return vcmd
def output_and_advance(self, **inputs):
try:
q = inputs['q']
dq = inputs['dq']
u = vectorops.div(vectorops.sub(inputs['qcmd'], q), inputs['dt'])
except KeyError:
print "Warning, cannot debug motion model, dq or dqcmd not in input"
return None
if self.dqpredlast != None:
if self.activeDofs != None:
dq = dq[:]
for i in [
i for i in range(len(q)) if i not in self.activeDofs
]:
dq[i] = self.dqpredlast[i]
#compare motion model to dq
print "Motion model error:", np.linalg.norm(self.dqpredlast -
np.array(dq))
(v, i) = max(
zip(
np.abs(self.dqpredlast - np.array(dq)).tolist(),
range(len(dq))))
print " Max error:", v, "at", i,
if self.robot != None: print self.robot.link(i).getName()
else: print
print " Command:", self.ulast[i], "Predicted:", self.dqpredlast[
i], "Actual:", dq[i]
print " pred:", self.Alast[i, i], "*u +", self.blast[i]
def getCommandVelocity(self, q, dq, dt): """Gets the command velocity from the current state of the robot. """ eP = self.getSensedError(q) #vcmd = hP*eP + hD*eV + hI*eI vP = gen_mul(self.hP,eP) vcmd = vP #D term vcur = self.getSensedVelocity(q,dq,dt) eD = None if vcur != None: eD = vectorops.sub(vcur, self.dxdes) vD = gen_mul(self.hD,eD) vcmd = vectorops.add(vcmd, vD) #I term if self.eI != None: vI = gen_mul(self.hI,self.eI) vcmd = vectorops.add(vcmd, vI) #print "task",self.name,"error P=",eP,"D=",eD,"E=",self.eI #do acceleration limiting if vcur != None: dvcmd = vectorops.div(vectorops.sub(vcmd,vcur),dt) dvnorm = vectorops.norm(dvcmd) if dvnorm > self.dvcmdmax: vcmd = vectorops.madd(vcur,dvcmd,self.dvcmdmax/dvnorm*dt) print self.name,"acceleration limited by factor",self.dvcmdmax/dvnorm*dt,"to",vcmd #do velocity limiting vnorm = vectorops.norm(vcmd) if vnorm > self.vcmdmax: vcmd = vectorops.mul(vcmd,self.vcmdmax/vnorm) print self.name,"velocity limited by factor",self.vcmdmax/vnorm,"to",vcmd return vcmd
def output_and_advance(self,**inputs):
try:
dt = inputs["dt"]
q = inputs["q"]
except KeyError:
raise ValueError("Input needs to have configuration 'q' and timestep 'dt'")
if len(q)==0: return None
if self.qlast==None:
dq = [0]*len(q)
else:
if self.robot==None:
dq = vectorops.div(self.robot.sub(q,self.qlast),dt)
else:
assert(len(self.qlast)==len(q))
dq = vectorops.div(self.robot.interpolate_deriv(self.qlast,q),dt)
self.qlast = q
return {'dq':dq}
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 advance(self, **inputs):
try:
dt = inputs["dt"]
q = inputs[self.name]
except KeyError:
raise ValueError(
"Input needs to have value '%s' and timestep 'dt'" %
(self.name, ))
if len(q) == 0: return None
if self.qlast == None:
dq = [0] * len(q)
else:
if self.robot == None:
dq = vectorops.div(self.robot.sub(q, self.qlast), dt)
else:
assert (len(self.qlast) == len(q))
dq = vectorops.div(self.robot.interpolate_deriv(self.qlast, q),
dt)
self.qlast = q
return {'d' + self.name: dq}
def calculate_workspace_axis(robot,obstacles,end_effector,point_local,axis_local,axis_world):
global lower_corner,upper_corner
resolution = (15,15,15)
cellsize = vectorops.div(vectorops.sub(upper_corner,lower_corner),resolution)
invcellsize = vectorops.div(resolution,vectorops.sub(upper_corner,lower_corner))
reachable = np.zeros(resolution)
#TODO: your code here
for i in range(resolution[0]):
for j in range(resolution[1]):
for k in range(resolution[2]):
orig_config = robot.getConfig()
point = vectorops.add(vectorops.mul([i,j,k], cellsize), lower_corner)
world_constraint = ik.fixed_rotation_objective(end_effector, world_axis=axis_world)
local_constraint = ik.fixed_rotation_objective(end_effector, local_axis=axis_local)
point_goal = ik.objective(end_effector, local=point_local, world=point)
if ik.solve_global([point_goal, local_constraint, world_constraint], iters=10):
reachable[i,j,k] = 1.0
robot.setConfig(orig_config)
return reachable
def drawDisconnections(orientation=None):
bmin, bmax = self.resolution.domain
active = [
i for i, (a, b) in enumerate(zip(bmin, bmax)[:3]) if b != a
]
if self.useboundary == None:
self.useboundary = (len(active) == 2)
verts, faces = self.resolution.computeDiscontinuities(
self.useboundary, orientation)
if len(active) == 3:
glEnable(GL_LIGHTING)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glDisable(GL_CULL_FACE)
#if self.walk_workspace_path != None:
# gldraw.setcolor(1,0,1,0.25)
#else:
# gldraw.setcolor(1,0,1,0.5)
for tri in faces:
tverts = [verts[v] for v in tri]
centroid = vectorops.div(vectorops.add(*tverts),
len(tri))
params = [
(x - a) / (b - a)
for (x, a,
b) in zip(centroid, self.resolution.domain[0],
self.resolution.domain[1])
]
if self.walk_workspace_path != None:
gldraw.setcolor(params[0], params[1], params[2],
0.25)
else:
gldraw.setcolor(params[0], params[1], params[2],
1.0)
gldraw.triangle(*tverts)
glEnable(GL_CULL_FACE)
else:
glDisable(GL_LIGHTING)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glColor4f(1, 0, 1, 0.5)
glLineWidth(4.0)
glBegin(GL_LINES)
for s in faces:
spts = verts[s[0]], verts[s[1]]
glVertex3f(spts[0][0], 0, spts[0][1])
glVertex3f(spts[1][0], 0, spts[1][1])
glEnd()
glLineWidth(1.0)
def getSensedVelocity(self, q, dq, dt):
"""Gets task velocity from sensed configuration q.
Default implementation uses finite differencing. Other
implementations may use jacobian.
"""
#uncomment this to get a jacobian based technique
#return np.dot(self.getJacobian(q),dq)
if self.qLast == None:
return None
else:
xlast = self.getSensedValue(self.qLast)
xcur = self.getSensedValue(q)
return vectorops.div(self.taskDifference(xcur, xlast), dt)
def getSensedVelocity(self, q, dq, dt): """Gets task velocity from sensed configuration q. Default implementation uses finite differencing. Other implementations may use jacobian. """ #uncomment this to get a jacobian based technique #return np.dot(self.getJacobian(q),dq) if self.qLast==None: return None else: xlast = self.getSensedValue(self.qLast) xcur = self.getSensedValue(q) return vectorops.div(self.taskDifference(xcur,xlast),dt)
def setDesiredVelocitiesFromDifference(self, qdes0, qdes1, dt, tasks=None):
"""Sets all the tasks' desired velocities from a given pair
of configurations separated by dt (e.g., to follow a reference
trajectory).
If the 'tasks' variable is provided, it should be a list of
tasks for which the desired values should be set.
"""
if tasks == None:
tasks = self.taskList
for t in tasks:
xdes0 = t.getSensedValue(qdes0)
xdes1 = t.getSensedValue(qdes1)
dx = vectorops.div(t.taskDifference(xdes1, xdes0), dt)
t.setDesiredVelocity(dx)
def calculate_workspace_free(robot, obstacles, end_effector, point_local):
"""Calculate the reachable workspace of the end effector point whose coordinates
are `point_local` on link `end_effector`. Ensure that the robot is collision
free with itself and with environment obstacles
"""
global lower_corner, upper_corner
resolution = (20, 20, 20)
cellsize = vectorops.div(vectorops.sub(upper_corner, lower_corner),
resolution)
invcellsize = vectorops.div(resolution,
vectorops.sub(upper_corner, lower_corner))
reachable = np.zeros(resolution)
#TODO: your code here
feasible = collision_free(robot, obstacles)
if feasible:
wp = end_effector.getWorldPosition(point_local)
index = [
int(math.floor(v)) for v in vectorops.mul(
vectorops.sub(wp, lower_corner), invcellsize)
]
if all(i >= 0 and i < r for (i, r) in zip(index, resolution)):
reachable[tuple(index)] = 1.0
return reachable
def setDesiredVelocitiesFromDifference(self,qdes0,qdes1,dt,tasks=None): """Sets all the tasks' desired velocities from a given pair of configurations separated by dt (e.g., to follow a reference trajectory). If the 'tasks' variable is provided, it should be a list of tasks for which the desired values should be set. """ if tasks == None: tasks = self.taskList for t in tasks: xdes0 = t.getSensedValue(qdes0) xdes1 = t.getSensedValue(qdes1) dx = vectorops.div(t.taskDifference(xdes1,xdes0),dt) t.setDesiredVelocity(dx)
def advance(self,**inputs):
try:
qcmd = inputs['qcmd']
q = inputs['q']
dt = inputs['dt']
except KeyError:
return
if len(q)==0: return
print len(qcmd),len(q)
u = vectorops.sub(qcmd,q)
u = vectorops.div(u,dt)
print "u estimate:",u
print "u estimate rarm:",u[26:33]
try:
q = inputs[self.xnames[0]]
dq = inputs[self.xnames[1]]
except KeyError as e:
print "Warning, inputs do not contain x key",e
return
"""
try:
u = sum((inputs[uname] for uname in self.unames),[])
except KeyError as e:
print "Warning, inputs do not contain u key",e
#u = [0]*(len(x)/2)
try:
u = vectorops.sub(inputs['qcmd'],inputs['q'])
u = vectorops.div(u,inputs['dt'])
except KeyError as e:
print "Warning, inputs do not contain qcmd key either",e
u = [0]*(len(x)/2)
"""
#self.xlast = x
#return
#do system ID
if self.qlast != None:
self.estimator.add(self.qlast,self.dqlast,u,q,dq)
pass
#update last state
self.qlast = q
self.dqlast = dq
return
def makeHapticCartesianPoseMsg(self):
deviceState = self.haptic_client.deviceState
if len(deviceState)==0:
print "No device state"
return None
transforms = [self.serviceThread.eeGetter_left.get(),self.serviceThread.eeGetter_right.get()]
update = False
commanding_arm=[0,0]
for i,dstate in enumerate(deviceState):
if dstate.buttonDown[0]:
commanding_arm[i]=1
if self.startTransforms[i] == None:
self.startTransforms[i] = transforms[i]
#RESET THE HAPTIC FORCE FEEDBACK CENTER
#self.serviceThread.hapticupdater.activateSpring(kP=hapticArmSpringStrength,kD=0,center=dstate.position,device=i)
#UPDATE THE HAPTIC FORCE FEEDBACK CENTER FROM ROBOT EE POSITION
#need to invert world coordinates to widget coordinates to haptic device coordinates
#widget and world translations are the same
dx = vectorops.sub(transforms[i][1],self.startTransforms[i][1]);
dxwidget = vectorops.div(dx,hapticArmTranslationScaling)
R,device_center = widgetToDeviceTransform(so3.identity(),vectorops.add(dxwidget,dstate.widgetInitialTransform[0][1]))
#print "Device position error",vectorops.sub(dstate.position,device_center)
if vectorops.distance(dstate.position,device_center) > 0.03:
c = vectorops.madd(device_center,vectorops.unit(vectorops.sub(dstate.position,device_center)),0.025)
self.serviceThread.hapticupdater.activateSpring(kP=hapticArmSpringStrength,kD=0,center=c,device=i)
else:
#deactivate
self.serviceThread.hapticupdater.activateSpring(kP=0,kD=0,device=i)
if dstate.newupdate:
update = True
else:
if self.startTransforms[i] != None:
self.serviceThread.hapticupdater.deactivateHapticFeedback(device=i)
self.startTransforms[i] = None
dstate.newupdate = False
if update:
msg = {}
msg['type'] = 'CartesianPose'
T1 = self.getDesiredCartesianPose('left',0)
R1,t1=T1
T2 = self.getDesiredCartesianPose('right',1)
R2,t2=T2
transforms = [(R1,t1),(R2,t2)]
if commanding_arm[0] and commanding_arm[1]:
msg['limb'] = 'both'
msg['position'] = t1+t2
msg['rotation'] = R1+R2
msg['maxJointDeviation']=0.5
msg['safe'] = int(CollisionDetectionEnabled)
self.CartesianPoseControl = True
return msg
elif commanding_arm[0] ==0:
msg['limb'] = 'right'
msg['position'] = t2
msg['rotation'] = R2
msg['maxJointDeviation']=0.5
msg['safe'] = int(CollisionDetectionEnabled)
self.CartesianPoseControl = True
return msg
else:
msg['limb'] = 'left'
msg['position'] = t1
msg['rotation'] = R1
msg['maxJointDeviation']=0.5
msg['safe'] = int(CollisionDetectionEnabled)
self.CartesianPoseControl = True
return msg
else:
return None
def signedDistance_gradient(self, point):
d = vectorops.sub(point, self.center)
return vectorops.div(d, vectorops.norm(d))
def display(self):
if self.configs == None:
self.robot.drawGL()
else:
for q in self.configs:
self.robot.setConfig(q)
self.robot.drawGL()
if self.walk_workspace_path != None:
if self.temp_config:
self.robot.setConfig(self.temp_config)
glEnable(GL_BLEND)
#glDisable(GL_DEPTH_TEST)
for i in xrange(self.robot.numLinks()):
self.robot.link(i).appearance().setColor(1, 0, 0, 0.5)
self.robot.drawGL()
for i in xrange(self.robot.numLinks()):
self.robot.link(i).appearance().setColor(0.5, 0.5, 0.5, 1)
#glEnable(GL_DEPTH_TEST)
glDisable(GL_BLEND)
glColor3f(1, 1, 0)
glLineWidth(5.0)
glBegin(GL_LINE_STRIP)
for w in self.walk_workspace_path.milestones:
if len(w) == 2:
glVertex2f(w[0], w[1])
else:
glVertex3f(w[0], w[1], w[2])
glEnd()
glLineWidth(1.0)
assert len(
self.resolution.ikTaskSpace) == 1, "Can only do one task for now"
task = self.resolution.ikTaskSpace[0]
#DEBUG
if hasattr(self.resolution, 'delaunay'):
delaunay = self.resolution.delaunay
delaunay_pts = self.resolution.delaunay_pts
delaunay_epts = self.resolution.delaunay_epts
delaunay_others = self.resolution.delaunay_others
ptset = set(tuple(v) for v in delaunay_pts)
assert len(ptset) == len(
delaunay_pts), "Duplicate points! %d unique / %d" % (
len(ptset), len(delaunay_pts))
glDisable(GL_LIGHTING)
glDisable(GL_DEPTH_TEST)
glColor3f(0, 0, 0)
glLineWidth(1.0)
import random
for s in delaunay.simplices:
"""
glBegin(GL_LINE_LOOP)
for v in s:
glVertex3fv(delaunay_pts[v])
glEnd()
"""
centroid = vectorops.div(
vectorops.add(*[delaunay_pts[v] for v in s]), len(s))
assert len(s) == 3
glBegin(GL_LINES)
for i, v in enumerate(s):
neighbors = [w for j, w in enumerate(s) if i < j]
for w in neighbors:
if (v in delaunay_epts and w in delaunay_others) or (
w in delaunay_epts and v in delaunay_others):
glColor3f(1, 1, 1)
else:
glColor3f(0, 0, 0)
glVertex3fv(
vectorops.interpolate(delaunay_pts[v], centroid,
0.1))
glVertex3fv(
vectorops.interpolate(delaunay_pts[w], centroid,
0.1))
glEnd()
glColor3f(0, 1, 0)
glPointSize(5.0)
glBegin(GL_POINTS)
for v in delaunay_epts:
glVertex3fv(delaunay_pts[v])
glEnd()
glColor3f(0, 0, 1)
glPointSize(5.0)
glBegin(GL_POINTS)
for v in delaunay_others:
glVertex3fv(delaunay_pts[v])
glEnd()
glEnable(GL_DEPTH_TEST)
#draw workspace graph
if self.drawWorkspaceRoadmap:
def drawWorkspaceRoadmap(orientation=None):
G = self.resolution.Gw
if orientation is not None:
Rclosest, G = self.resolution.extractOrientedSubgraph(
orientation)
print G.number_of_nodes()
if not self.resolution.hasResolution():
glDisable(GL_LIGHTING)
glPointSize(5.0)
glColor3f(1, 0, 0)
glBegin(GL_POINTS)
for n, d in G.nodes_iter(data=True):
glVertex3fv(self.workspaceToPoint(d['params']))
glEnd()
glColor3f(1, 0.5, 0)
glBegin(GL_LINES)
for (i, j) in G.edges_iter():
glVertex3fv(self.workspaceToPoint(G.node[i]['params']))
glVertex3fv(self.workspaceToPoint(G.node[j]['params']))
glEnd()
else:
#maxn = max(len(d['qlist']) for n,d in G.nodes_iter(data=True))
#maxe = max(len(d['qlist']) for i,j,d in G.edges_iter(data=True))
#maxn = max(maxn,1)
#maxe = max(maxe,1)
glDisable(GL_LIGHTING)
glPointSize(5.0)
glBegin(GL_POINTS)
for n, d in G.nodes_iter(data=True):
if not self.resolution.isResolvedNode(n):
continue
#u = float(len(d['qlist']))/float(maxn)
#nsubset = sum(1 for iq in d['qlist'] if iq in self.rr.Qsubgraph)
#if nsubset > 1:
# glColor3f(1,0,1)
#else:
# glColor3f(u,nsubset*0.5,0)
glColor3f(1, 0.5, 0)
glVertex3fv(self.workspaceToPoint(d['params']))
glEnd()
glBegin(GL_LINES)
for (i, j, d) in G.edges_iter(data=True):
if not self.resolution.isResolvedNode(
i) or not self.resolution.isResolvedNode(j):
continue
#nsubset = sum(1 for (ia,ib) in d['qlist'] if (ia in self.Qsubgraph and ib in self.Qsubgraph))
#u = float(len(d['qlist']))/float(maxe)
r, g, b = 1, 1, 0
if not self.resolution.isResolvedEdge(i, j):
r, g, b = 1, 0, 1
else:
qd = self.robot.distance(G.node[i]['config'],
G.node[j]['config'])
wd = self.resolution.workspaceDistance(
G.node[i]['params'], G.node[j]['params'])
u = 1.0 - math.exp(-max(0.0, 2.0 * qd / wd - 1.0))
if u > 0.8:
r, g, b = 1, 1.0 - u * 5.0, 0.0
else:
r, g, b = u / 0.8, 1, 0.0
#assert (nsubset >=1) == self.resolution.isResolvedEdge(i,j),"Mismatch between Qsubgraph and resolution?"
glColor3f(r, g, b)
glVertex3fv(self.workspaceToPoint(G.node[i]['params']))
glVertex3fv(self.workspaceToPoint(G.node[j]['params']))
glEnd()
"""
glEnable(GL_LIGHTING)
q0 = self.robot.getConfig()
for iw,d in self.rr.Gw.nodes_iter(data=True):
qs = [iq for iq in d['qlist'] if iq in self.rr.Qsubgraph]
if len(qs) > 1:
for iq in qs:
self.robot.setConfig(self.rr.Gq.node[iq]['config'])
self.robot.drawGL()
self.robot.setConfig(q0)
glDisable(GL_LIGHTING)
"""
if task.numConstraints > 3:
if not hasattr(self, 'roadmapDisplayLists_by_orientation'):
self.roadmapDisplayLists_by_orientation = dict()
self.lastROrientation = None
self.lastRMbest = None
orientation = self.xformWidget.get()[0]
if orientation != self.lastROrientation:
mbest = self.resolution.closestOrientation(orientation)
self.lastRMbest = mbest
else:
mbest = self.lastRMbest
if mbest not in self.roadmapDisplayLists_by_orientation:
print "Drawing new display list for moment", mbest
self.roadmapDisplayLists_by_orientation[
mbest] = CachedGLObject()
self.roadmapDisplayLists_by_orientation[mbest].draw(
drawWorkspaceRoadmap, args=(orientation, ))
else:
#there looks to be a bug in GL display lists for drawing lines...
#self.roadmapDisplayList.draw(drawWorkspaceRoadmap)
drawWorkspaceRoadmap()
else:
#render boundaries only
def drawDisconnections(orientation=None):
bmin, bmax = self.resolution.domain
active = [
i for i, (a, b) in enumerate(zip(bmin, bmax)[:3]) if b != a
]
if self.useboundary == None:
self.useboundary = (len(active) == 2)
verts, faces = self.resolution.computeDiscontinuities(
self.useboundary, orientation)
if len(active) == 3:
glEnable(GL_LIGHTING)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glDisable(GL_CULL_FACE)
#if self.walk_workspace_path != None:
# gldraw.setcolor(1,0,1,0.25)
#else:
# gldraw.setcolor(1,0,1,0.5)
for tri in faces:
tverts = [verts[v] for v in tri]
centroid = vectorops.div(vectorops.add(*tverts),
len(tri))
params = [
(x - a) / (b - a)
for (x, a,
b) in zip(centroid, self.resolution.domain[0],
self.resolution.domain[1])
]
if self.walk_workspace_path != None:
gldraw.setcolor(params[0], params[1], params[2],
0.25)
else:
gldraw.setcolor(params[0], params[1], params[2],
1.0)
gldraw.triangle(*tverts)
glEnable(GL_CULL_FACE)
else:
glDisable(GL_LIGHTING)
glEnable(GL_BLEND)
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA)
glColor4f(1, 0, 1, 0.5)
glLineWidth(4.0)
glBegin(GL_LINES)
for s in faces:
spts = verts[s[0]], verts[s[1]]
glVertex3f(spts[0][0], 0, spts[0][1])
glVertex3f(spts[1][0], 0, spts[1][1])
glEnd()
glLineWidth(1.0)
#draw stabbing lines
"""
glDisable(GL_LIGHTING)
glColor3f(1,0,0)
glBegin(GL_LINES)
for (i,j,d) in self.resolution.Gw.edges_iter(data=True):
if self.resolution.isResolvedEdge(i,j):
continue
if not self.resolution.isResolvedNode(i) or not self.resolution.isResolvedNode(j):
continue
glVertex3fv(self.workspaceToPoint(self.resolution.Gw.node[i]['params']))
glVertex3fv(self.workspaceToPoint(self.resolution.Gw.node[j]['params']))
glEnd()
"""
if task.numConstraints > 3:
if not hasattr(self,
'disconnectionDisplayLists_by_orientation'):
self.disconnectionDisplayLists_by_orientation = dict()
self.lastOrientation = None
self.lastMbest = None
orientation = self.xformWidget.get()[0]
if orientation != self.lastOrientation:
mbest = self.resolution.closestOrientation(orientation)
self.lastMbest = mbest
else:
mbest = self.lastMbest
if mbest not in self.disconnectionDisplayLists_by_orientation:
self.disconnectionDisplayLists_by_orientation[
mbest] = CachedGLObject()
self.disconnectionDisplayLists_by_orientation[mbest].draw(
drawDisconnections, args=(mbest, ))
else:
self.disconnectionDisplayList.draw(drawDisconnections)
bmin, bmax = self.resolution.domain
#draw workspace bound
glDisable(GL_LIGHTING)
glColor3f(1, 0.5, 0)
gldraw.box(bmin[:3], bmax[:3], lighting=False, filled=False)
GLWidgetPlugin.display(self)
def find_target(self):
self.target_prep_pos = (so3.identity(), [0, 10, -1])
count = 0
# print self.waiting_list
for idx in self.waiting_list:
# print 'i',idx
bb = self.sim.world.rigidObject(idx).geometry().getBB()
#decide the hand facing and finger configuration
z_limit = 0.65
ratio = 1.0
if bb[1][2] > z_limit:
hand_facing = 'face_toward_shelf'
count += 1
else:
hand_facing = 'face_down'
count += 1
if bb[1][2] < 0.54 and self.hand_facing != 'sweep':
hand_facing = 'sweep'
if hand_facing == 'face_toward_shelf':
max_limit = forward_max
min_limit = forward_min
face = face_toward_shelf
hand_mode = 'power'
elif hand_facing == 'face_down':
max_limit = face_down_max
min_limit = face_down_min
x_diff = bb[1][0] - bb[0][0]
y_diff = bb[1][1] - bb[0][1]
if y_diff > x_diff * ratio:
hand_mode = 'power'
face = face_down
elif x_diff > y_diff * ratio:
hand_mode = 'power'
face = face_down_rot
else:
hand_mode = 'precision'
face = face_down
else:
face = sweep_pos
hand_mode = 'power'
if hand_facing == 'face_down':
target_prep_pos = (face,
vectorops.add(
vectorops.div(
vectorops.add(bb[1], bb[0]), 2),
[0, 0, 0.5]))
top_bb = bb[1][2]
target_pos = (face,
vectorops.div(vectorops.add(bb[1], bb[0]), 2))
target_pos[1][2] = top_bb + 0.06
for i in range(3):
target_prep_pos[1][i] = max(
min(target_prep_pos[1][i], max_limit[i]), min_limit[i])
target_pos[1][i] = max(min(target_pos[1][i], max_limit[i]),
min_limit[i])
elif hand_facing == 'face_toward_shelf':
target_prep_pos = (face,
vectorops.add(
vectorops.div(
vectorops.add(bb[1], bb[0]), 2),
[0, 0, 0.02]))
back_bb = bb[0][1]
target_pos = (face,
vectorops.div(vectorops.add(bb[1], bb[0]), 2))
target_prep_pos[1][1] = back_bb - 0.07
target_pos[1][1] = back_bb - 0.04
for i in range(3):
target_prep_pos[1][i] = max(
min(target_prep_pos[1][i], max_limit[i]), min_limit[i])
target_pos[1][i] = max(min(target_pos[1][i], max_limit[i]),
min_limit[i])
else:
target_prep_pos = (face, [(bb[1][0] + bb[0][0]) * 0.5, 0.55,
0.66])
y_len = bb[1][1] - bb[0][1]
target_pos = (face, [(bb[1][0] + bb[0][0]) * 0.5, 0.92 - y_len,
0.66])
if count > 0 and hand_facing == 'sweep':
continue
if target_prep_pos[1][
1] + self.num_pick[idx] * 0.01 < self.target_prep_pos[1][
1] or self.hand_facing == 'sweep':
self.hand_facing = hand_facing
self.hand_mode = hand_mode
self.target_prep_pos = target_prep_pos
self.target_pos = target_pos
self.target_id = idx
self.num_pick[self.target_id] += 1
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 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 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
def predict_line(lineStarts,lineEnds,lineNormals,lineTorqueAxes,pcd,param,discretization,num_iter,queryDList,vis,world):
vision_task_1 = False#True means we are doing the collision detection.
o3d = False
#create a pcd in open3D
equilibriumPcd = open3d.geometry.PointCloud()
xyz = []
rgb = []
normals = []
for ele in pcd:
xyz.append(ele[0:3])
rgb.append(ele[3:6])
normals.append(ele[6:9])
equilibriumPcd.points = open3d.utility.Vector3dVector(np.asarray(xyz,dtype=np.float32))
equilibriumPcd.colors = open3d.utility.Vector3dVector(np.asarray(rgb,dtype=np.float32))
equilibriumPcd.normals = open3d.utility.Vector3dVector(np.asarray(normals,dtype=np.float32))
# equilibriumPcd,ind=statistical_outlier_removal(equilibriumPcd,nb_neighbors=20,std_ratio=0.75)
if o3d:
vis3 = open3d.visualization.Visualizer()
vis3.create_window()
open3dPcd = open3d.geometry.PointCloud()
vis3.add_geometry(open3dPcd)
probe = world.rigidObject(0)
klampt_pcd_object = world.rigidObject(1) #this is a rigid object
klampt_pcd = klampt_pcd_object.geometry().getPointCloud() #this is now a PointCloud()
N_pts = klampt_pcd.numPoints()
dt = 0.1
for pointNumber in num_iter:
####Preprocess the pcd ####
lineStart0 = lineStarts[pointNumber]
lineEnd0 =lineEnds[pointNumber]
lineTorqueAxis = lineTorqueAxes[pointNumber]
N = 1 + round(vo.norm(vo.sub(lineStart0,lineEnd0))/discretization)
s = np.linspace(0,1,N)
lineNormal = lineNormals[pointNumber]
localXinW = vo.sub(lineEnd0,lineStart0)/np.linalg.norm(vo.sub(lineEnd0,lineStart0))
localYinW = (lineTorqueAxis)/np.linalg.norm(lineTorqueAxis)
lineStartinLocal = [0,0]
lineEndinLocal = [np.dot(vo.sub(lineEnd0,lineStart0),localXinW),
np.dot(vo.sub(lineEnd0,lineStart0),localYinW)]
#################first project the pcd to the plane of the line##############
#The start point is the origin of the plane...
projectedPcd = [] #Each point is R^10
projectedPcdLocal = [] #localXY coordinate
#the projected Pcd is in local frame....
for i in range(len(pcd)):
p = pcd[i][0:3]
projectedPt = vo.sub(p,vo.mul(lineNormal,vo.dot(vo.sub(p,lineStart0),lineNormal))) ##world origin
projectedPt2D = vo.sub(projectedPt,lineStart0)#world origin
projectedPt2DinLocal = [vo.dot(projectedPt2D,localXinW),vo.dot(projectedPt2D,localYinW)]
#%make sure point is in the "box" defined by the line
if ((projectedPt2DinLocal[0]<0.051) and (projectedPt2DinLocal[0] > -0.001)
and (projectedPt2DinLocal[1]<0.001) and (projectedPt2DinLocal[1]>-0.001)):
projectedPcdLocal.append(projectedPt2DinLocal)
projectedPcd.append(pcd[i])
#Create a KDTree for searching
projectedPcdTree = spatial.KDTree(projectedPcdLocal)
###############Find the corresponding point on the surface to the line############
surfacePtsAll = []# %These are the surface pts that will be displaced.
#%part of the probe not in contact with the object...
#average 3 neighbors
NofN = 3
#rigidPointsFinal = [] ##we have a potential bug here for not using rigidPointsFinal....
for i in range(int(N)):
tmp =vo.mul(vo.sub(lineEnd0,lineStart0),s[i])#%the point on the line, projected
linePt = [vo.dot(tmp,localXinW),vo.dot(tmp,localYinW)]
d,Idx = projectedPcdTree.query(linePt[0:2],k=NofN)
#We might end up having duplicated pts...
#We should make sure that the discretization is not too fine..
#or should average a few neighbors
if d[0] < 0.002:
surfacePt = [0]*10
for j in range(NofN):
surfacePt = vo.add(surfacePt,projectedPcd[Idx[j]][0:10])
surfacePt = vo.div(surfacePt,NofN)
surfacePtsAll.append(surfacePt) #position in the global frame..
#rigidPointsFinal.append(linePts)
N = len(surfacePtsAll)
##### Preprocesss done
if not o3d:
vis.show()
time.sleep(15.0)
############# Go through a list of displacements
totalFinNList = []
#for queryD = -0.003:0.001:0.014
for queryD in queryDList:
lineStart = vo.sub(lineStart0,vo.mul(lineNormal,queryD))
lineEnd = vo.sub(lineEnd0,vo.mul(lineNormal,queryD))
lineCenter = vo.div(vo.add(lineStart,lineEnd),2)
torqueCenter = vo.add(lineCenter,vo.mul(lineNormal,0.09))
if vision_task_1:
vis.lock()
print(queryD)
x_axis = vo.mul(vo.unit(vo.sub(lineEnd,lineStart)),-1.0)
y_axis = [lineTorqueAxis[0],lineTorqueAxis[1],lineTorqueAxis[2]]
z_axis = vo.cross(x_axis,y_axis)
z_axis = [z_axis[0],z_axis[1],z_axis[2]]
R = x_axis+y_axis+z_axis
t = vo.add(lineCenter,vo.mul(lineNormal,0.002))
print(R,t)
probe.setTransform(R,t)
vis.unlock()
time.sleep(dt)
else:
if not o3d:
vis.lock()
print(queryD)
x_axis = vo.mul(vo.unit(vo.sub(lineEnd,lineStart)),-1.0)
y_axis = [lineTorqueAxis[0],lineTorqueAxis[1],lineTorqueAxis[2]]
z_axis = vo.cross(x_axis,y_axis)
z_axis = [z_axis[0],z_axis[1],z_axis[2]]
R = x_axis+y_axis+z_axis
t = vo.add(lineCenter,vo.mul(lineNormal,0.003))
probe.setTransform(R,t)
####calculate the nominal displacements
surfacePts = [] #These are the surface pts that will be displaced...
rigidPtsInContact = []
nominalD = [] #Column Vector..
for i in range(int(N)): ##we have a potential bug here for keep interating trhough N, since about, if d[0] < 0.002, the points is not added...
#weird no bug occured...
linePt = vo.add(vo.mul(vo.sub(lineEnd,lineStart),s[i]),lineStart)
surfacePt = surfacePtsAll[i][0:3]
normal = surfacePtsAll[i][6:9]
nominalDisp = -vo.dot(vo.sub(linePt,surfacePt),normal)
if nominalDisp > 0:
surfacePts.append(surfacePtsAll[i][0:10])#position in the global frame..
rigidPtsInContact.append(linePt)
nominalD.append(nominalDisp)
originalNominalD = deepcopy(nominalD)
NofSurfacePts = len(surfacePts)
if NofSurfacePts > 0:
negativeDisp = True
while negativeDisp:
NofSurfacePts = len(surfacePts)
K = np.zeros((NofSurfacePts,NofSurfacePts))
for i in range(NofSurfacePts):
for j in range(NofSurfacePts):
K[i][j] = invKernel(surfacePts[i][0:3],surfacePts[j][0:3],param)
#print K
#startTime = time.time()
actualD = np.dot(np.linalg.inv(K),nominalD)
#print('Time spent inverting matrix',time.time()- startTime)
#print nominalD,actualD
negativeIndex = actualD < 0
if np.sum(negativeIndex) > 0:
actualD = actualD.tolist()
surfacePts = [surfacePts[i] for i in range(len(surfacePts)) if actualD[i]>=0]
nominalD = [nominalD[i] for i in range(len(nominalD)) if actualD[i]>=0]
rigidPtsInContact = [rigidPtsInContact[i] for i in range(len(rigidPtsInContact)) if actualD[i]>=0]
else:
negativeDisp = False
#hsv's hue: 0.25 = 0.0002 displacement 1 == 10 displacement
#generate the entire displacement field
#surfacePts are the equlibrium pts that provide
entireDisplacedPcd = []
colorThreshold = 0.0005
colorUpperBound = 0.012
forceColorUpperBound = 0.004
colorRange = colorUpperBound - colorThreshold
greyColor = 0.38
xyz = []
rgb = []
xyzForce = []
rgbForce = []
maxDisp = 0
for pt,ptNormal in zip(equilibriumPcd.points,equilibriumPcd.normals):
shapeVector = []
for pt2 in surfacePts:
shapeVector.append(invKernel(pt[0:3],pt2[0:3],param))
nominalDisplacement = vo.dot(shapeVector,actualD)
if nominalDisplacement > maxDisp:
maxDisp = nominalDisplacement
color = colorsys.hsv_to_rgb(nominalDisplacement/colorRange*0.75+0.25,1,0.75)
color = [color[0],color[1],color[2]]
displacedDeformablePoint = vo.sub(pt[0:3],vo.mul(ptNormal,nominalDisplacement))
xyz.append(displacedDeformablePoint)
rgb.append(color)
counter = 0
for (p,c) in zip(xyz,equilibriumPcd.colors):#rgb):
klampt_pcd.setProperty(counter,'r',c[0])
klampt_pcd.setProperty(counter,'g',c[1])
klampt_pcd.setProperty(counter,'b',c[2])
klampt_pcd.setPoint(counter,p)
counter = counter + 1
klampt_pcd_object.geometry().setPointCloud(klampt_pcd)
if o3d:
open3dPcd.points = open3d.utility.Vector3dVector(np.asarray(xyz,dtype=np.float32))
open3dPcd.colors = open3d.utility.Vector3dVector(np.asarray(rgb,dtype=np.float32))
open3dPcd.estimate_normals(search_param = open3d.geometry.KDTreeSearchParamHybrid(radius = 0.1, max_nn = 30))
###open3dPcd,ind=statistical_outlier_removal(open3dPcd,nb_neighbors=10,std_ratio=2)
#open3d.visualization.draw_geometries([open3dPcd])#_box_bottom])
vis3.add_geometry(open3dPcd)
vis3.poll_events()
vis3.update_renderer()
if not o3d:
vis.unlock()
time.sleep(dt)
else:
pass
while vis.shown():
time.sleep(0.1)
return
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
