def apply_tendon_forces(self,i,link1,link2,rest_length):
tendon_c2 = 30000.0
tendon_c1 = 10000.0
b0 = self.sim.body(self.model.robot.link(self.model.proximal_links[0]-3))
b1 = self.sim.body(self.model.robot.link(link1))
b2 = self.sim.body(self.model.robot.link(link2))
p0w = se3.apply(b1.getTransform(),self.tendon0_local)
p1w = se3.apply(b1.getTransform(),self.tendon1_local)
p2w = se3.apply(b2.getTransform(),self.tendon2_local)
d = vectorops.distance(p1w,p2w)
if d > rest_length:
#apply tendon force
direction = vectorops.unit(vectorops.sub(p2w,p1w))
f = tendon_c2*(d - rest_length)**2+tendon_c1*(d - rest_length)
#print d,rest_length
#print "Force magnitude",self.model.robot.link(link1).getName(),":",f
f = min(f,100)
#tendon routing force
straight = vectorops.unit(vectorops.sub(p2w,p0w))
pulley_direction = vectorops.unit(vectorops.sub(p1w,p0w))
pulley_axis = so3.apply(b1.getTransform()[0],(0,1,0))
tangential_axis = vectorops.cross(pulley_axis,pulley_direction)
cosangle = vectorops.dot(straight,tangential_axis)
#print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
base_direction = so3.apply(b0.getTransform()[0],[0,0,-1])
b0.applyForceAtLocalPoint(vectorops.mul(base_direction,-f),vectorops.madd(p0w,base_direction,0.04))
b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,cosangle*f),self.tendon1_local)
b1.applyForceAtLocalPoint(vectorops.mul(tangential_axis,-cosangle*f),self.tendon0_local)
b2.applyForceAtLocalPoint(vectorops.mul(direction,-f),self.tendon2_local)
self.forces[i][1] = vectorops.mul(tangential_axis,cosangle*f)
self.forces[i][2] = vectorops.mul(direction,f)
else:
self.forces[i] = [None,None,None]
return
def random_rotation():
"""Returns a uniformly distributed rotation matrix."""
q = [random.gauss(0,1),random.gauss(0,1),random.gauss(0,1),random.gauss(0,1)]
q = vectorops.unit(q)
theta = math.acos(q[3])*2.0
if abs(theta) < 1e-8:
m = [0,0,0]
else:
m = vectorops.mul(vectorops.unit(q[0:3]),theta)
return so3.from_moment(m)
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 df_dz(self, x, y, z):
n = len(y)
m = len(z) // n
result = np.zeros((6, len(z)))
T = self.env_geom.getTransform()
CoM = se3.apply(T, self.env_obj.getMass().getCom())
for i in range(n):
point = y[i][1]
Normal_normal = normal(self.env_geom, point)
n1 = so3.canonical(Normal_normal)[3:6]
n2 = so3.canonical(Normal_normal)[6:9]
Normal_friction = []
for j in range(6):
n_tmp = (math.cos((math.pi / 3) * j) * np.array(n1) + math.sin(
(math.pi / 3) * j) * np.array(n2)).tolist()
Normal_friction.append(vectorops.unit(n_tmp))
Cross_Normal = list(
vectorops.cross(vectorops.sub(point, CoM), Normal_normal))
Cross_Normal_friction = []
for j in range(6):
cross_Normal_v = list(
vectorops.cross(vectorops.sub(point, CoM),
Normal_friction[j]))
Cross_Normal_friction.append(cross_Normal_v)
result[0:3, i * m:(i + 1) * m - 1] = np.asarray(Normal_friction).T
result[0:3, (i + 1) * m - 1:(i + 1) *
m] = np.asarray(Normal_normal).reshape((3, 1))
result[3:6,
i * m:(i + 1) * m - 1] = np.asarray(Cross_Normal_friction).T
result[3:6, (i + 1) * m - 1:(i + 1) *
m] = np.asarray(Cross_Normal).reshape((3, 1))
return result
def segment_triangle_intersection(seg, tri, umin=0, umax=1):
"""Given a 3D segment (p,q) and a triangle (a,b,c), returns the point of
intersection if the region of the segment in interval [umin,umax] intersects the
triangle. Otherwise returns None"""
p, q = seg
a, b, c = tri
d = vectorops.sub(b, a)
e = vectorops.sub(c, a)
n = vectorops.cross(d, e)
if vectorops.norm(n) < 1e-7: #degenerate
return None
n = vectorops.unit(n)
ofs = vectorops.dot(n, a)
#Solve for n^T(p + u(q-p)) = ofs
denom = vectorops.dot(n, vectorops.sub(q, p))
numer = ofs - vectorops.dot(n, p)
if abs(denom) < 1e-7: #near parallel
return None
u = numer / denom
if u < umin or u > umax:
return None
#now check whether point of intersection lies in triangle
x = vectorops.madd(p, vectorops.sub(q, p), u)
xloc = vectorops.sub(x, a)
#solve for coordinates [r,s,t] such that xloc = r*n + s*d + t*e
try:
rst = np.dot(np.linalg.inv(np.array([n, d, e]).T), xloc)
except np.linalg.linalg.LinAlgError:
return None
r, s, t = rst
assert abs(r) < 1e-4
if s >= 0 and t >= 0 and s + t <= 1:
return x
return None
def grasp_from_contacts(contact1,contact2):
"""Helper: if you have two contacts, this returns an AntipodalGrasp"""
d = vectorops.unit(vectorops.sub(contact2.x,contact1.x))
grasp = AntipodalGrasp(vectorops.interpolate(contact1.x,contact2.x,0.5),d)
grasp.finger_width = vectorops.distance(contact1.x,contact2.x)
grasp.contact1 = contact1
grasp.contact2 = contact2
return grasp
def rand_rotation():
q = [
random.gauss(0, 1),
random.gauss(0, 1),
random.gauss(0, 1),
random.gauss(0, 1)
]
q = vectorops.unit(q)
return so3.from_quaternion(q)
def apply_tendon_forces(self, i, link1, link2, rest_length):
tendon_c2 = 30000.0
tendon_c1 = 10000.0
b0 = self.sim.body(
self.model.robot.link(self.model.proximal_links[0] - 3))
b1 = self.sim.body(self.model.robot.link(link1))
b2 = self.sim.body(self.model.robot.link(link2))
p0w = se3.apply(b1.getTransform(), self.tendon0_local)
p1w = se3.apply(b1.getTransform(), self.tendon1_local)
p2w = se3.apply(b2.getTransform(), self.tendon2_local)
d = vectorops.distance(p1w, p2w)
if d > rest_length:
#apply tendon force
direction = vectorops.unit(vectorops.sub(p2w, p1w))
f = tendon_c2 * (d - rest_length)**2 + tendon_c1 * (d -
rest_length)
#print d,rest_length
#print "Force magnitude",self.model.robot.link(link1).getName(),":",f
f = min(f, 100)
#tendon routing force
straight = vectorops.unit(vectorops.sub(p2w, p0w))
pulley_direction = vectorops.unit(vectorops.sub(p1w, p0w))
pulley_axis = so3.apply(b1.getTransform()[0], (0, 1, 0))
tangential_axis = vectorops.cross(pulley_axis, pulley_direction)
cosangle = vectorops.dot(straight, tangential_axis)
#print "Cosine angle",self.model.robot.link(link1).getName(),cosangle
base_direction = so3.apply(b0.getTransform()[0], [0, 0, -1])
b0.applyForceAtLocalPoint(
vectorops.mul(base_direction, -f),
vectorops.madd(p0w, base_direction, 0.04))
b1.applyForceAtLocalPoint(
vectorops.mul(tangential_axis, cosangle * f),
self.tendon1_local)
b1.applyForceAtLocalPoint(
vectorops.mul(tangential_axis, -cosangle * f),
self.tendon0_local)
b2.applyForceAtLocalPoint(vectorops.mul(direction, -f),
self.tendon2_local)
self.forces[i][1] = vectorops.mul(tangential_axis, cosangle * f)
self.forces[i][2] = vectorops.mul(direction, f)
else:
self.forces[i] = [None, None, None]
return
def df_dx(self, q, index_pt):
link_idx, pt_obj = index_pt
dist, point, _ = self.robot.geometry[link_idx].distance(pt_obj)
if dist > 1e-3:
worlddir = vectorops.unit(vectorops.sub(point, pt_obj))
#worlddir2 = self.robot.geometry[link_idx].normal(point)
#worlddir3 = vectorops.mul(normal(self.obj,pt_obj),-1.0)
#print("Comparing normals: {} vs {} vs {}".format('%.2f %.2f %.2f'%tuple(worlddir),'%.2f %.2f %.2f'%tuple(worlddir2),'%.2f %.2f %.2f'%tuple(worlddir3)))
else:
worlddir = self.robot.geometry[link_idx].normal(point)
#worlddir = vectorops.mul(normal(self.obj,pt_obj),-1.0)
Jp = self.robot.robot.link(link_idx).getPositionJacobian(
self.robot.robot.link(link_idx).getLocalPosition(point[0:3]))
return np.dot(np.array(Jp).T, worlddir) * self.scale
def force(q, target, obstacles):
"""Returns the potential field force for a robot at configuration q,
trying to reach the given target, with the specified obstacles.
Input:
- q: a 2D point giving the robot's configuration
- robotRadius: the radius of the robot
- target: a 2D point giving the robot's target
- obstacles: a list of Circle's giving the obstacles
"""
#basic target-tracking potential field implemented here
# calculate the attractive force due to the goal
f = vectorops.mul(vectorops.sub(target, q), attractiveConstant)
for obstacle in obstacles:
dist = obstacle.distance(q)
# only add the repulsive force if the obstacle is "within" range
if dist > repulsiveDistance:
continue
magnitude = (1.0 / dist - 1 / repulsiveDistance)
# alter the magnitude to have a repulsiveConstant and to
# divide by square of repulsiveDistance
# Dividing by the square strengthens the repulsive force
# The repuslvieConstant scales the strength of the repulsive force linearly
magnitude = repulsiveConstant * magnitude / (dist**2)
# set the direction of repulsive force away from the obstacle
direction = vectorops.unit(vectorops.sub(q, obstacle.center))
f = vectorops.madd(f, direction, magnitude)
#limit the norm of f
if vectorops.norm(f) > 1:
f = vectorops.unit(f)
return f
def value(self, x, y, z):
result = np.zeros(6)
if len(y) == 0:
result[0:3] += self.mass * self.gravity_coefficient * np.asarray(
self.gravity_direction)
return result
n = len(y)
m = len(z) // n
assert m == 7
T = self.env_geom.getTransform()
CoM = se3.apply(T, self.env_obj.getMass().getCom())
for i in range(n):
point = y[i][1]
f_normal = z[m * (i + 1) - 1]
f_friction = z[m * i:m * (i + 1) - 1]
Normal_normal = normal(self.env_geom, point)
n1 = so3.canonical(Normal_normal)[3:6]
n2 = so3.canonical(Normal_normal)[6:9]
Normal_friction = []
for j in range(6):
n_tmp = (math.cos((math.pi / 3) * j) * np.array(n1) + math.sin(
(math.pi / 3) * j) * np.array(n2)).tolist()
Normal_friction.append(vectorops.unit(n_tmp))
Cross_Normal = list(
vectorops.cross(vectorops.sub(point, CoM), Normal_normal))
Cross_Normal_friction = []
for j in range(6):
cross_Normal_v = list(
vectorops.cross(vectorops.sub(point, CoM),
Normal_friction[j]))
Cross_Normal_friction.append(cross_Normal_v)
result[0:3] += np.asarray(Normal_normal) * f_normal + np.asarray(
f_friction) @ np.asarray(Normal_friction)
result[3:6] += np.asarray(Cross_Normal) * f_normal + np.asarray(
f_friction) @ np.asarray(Cross_Normal_friction)
result[0:3] += self.mass * self.gravity_coefficient * np.asarray(
self.gravity_direction)
return result
def force(q, target, obstacles):
"""Returns the potential field force for a robot at configuration q,
trying to reach the given target, with the specified obstacles.
Input:
- q: a 2D point giving the robot's configuration
- robotRadius: the radius of the robot
- target: a 2D point giving the robot's target
- obstacles: a list of Circle's giving the obstacles
"""
#basic target-tracking potential field implemented here
#TODO: implement your own potential field
att_force = vectorops.mul(vectorops.sub(target, q), attractiveConstant)
total_rep_force = [0, 0]
for each_ob in obstacles:
cur_rep_force = repForce(q, each_ob, repulsiveDistance)
total_rep_force = vectorops.add(total_rep_force, cur_rep_force)
total_force = vectorops.add(att_force, total_rep_force)
#limit the norm of f
if vectorops.norm(total_force) > 1:
total_force = vectorops.unit(vectorops.add(total_force,
(1e-10, 1e-10)))
return total_force
def repForce(point_loc, in_obstacle, max_detect_dist):
'''
return the replusive force
according to the distance between the point and the obstacle
if it is too far away just return 0:
point_loc: (x,y)
in_obstacle: circle object
'''
obs_center = in_obstacle.getCenter()
dist = in_obstacle.distance(point_loc)
# if dist <= 0:
# return(0,0)
# #assert("Check current point location, overlaps the obstacle")
if dist > max_detect_dist:
return (0, 0)
else:
# direction to the obstacle center
dir_center = vectorops.sub(point_loc, obs_center)
# normalize vector
dir_center = vectorops.unit(dir_center)
# to avoid local min add a small tangential vector
# dir_around = (dir_center[1],dir_center[0])
return vectorops.mul(dir_center, 1 / abs(dist))
def sphere_grid(N):
"""Returns a nx.Graph describing a grid on S2 with resolution N.
The resulting graph has ~6N^2 nodes
The node attribute 'params' gives the point.
"""
assert N >= 1
G = nx.Graph()
namemap = dict()
for ax in range(3):
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)
idx = [i, j]
idx = idx[:ax] + [N] + idx[ax:]
idx = tuple(idx)
if idx in namemap:
continue
else:
namemap[idx] = idx
x = [u, v]
x = x[:ax] + [1.0] + x[ax:]
x = vectorops.unit(x)
G.add_node(idx, params=x)
idx = [i, j]
idx = idx[:ax] + [0] + idx[ax:]
idx = tuple(idx)
if idx in namemap:
continue
else:
namemap[idx] = idx
x = [u, v]
x = x[:ax] + [-1.0] + x[ax:]
x = vectorops.unit(x)
G.add_node(idx, params=x)
for ax in range(3):
for i in xrange(N + 1):
for j in xrange(N + 1):
baseidx = [i, j]
idx = baseidx[:ax] + [N] + baseidx[ax:]
idx = tuple(idx)
for n in range(2):
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])
idx = baseidx[:ax] + [0] + baseidx[ax:]
idx = tuple(idx)
for n in range(2):
nidx = baseidx[:]
nidx[n] += 1
if nidx[n] > N: continue
nidx = nidx[:ax] + [0] + nidx[ax:]
nidx = tuple(nidx)
G.add_edge(namemap[idx], namemap[nidx])
return G
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 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 run_poking_line_probe(config,tableHeight,probeLength,forceLimit,dt,moveStep,shortServoTime,longServoTime,
IKErrorTolerence,maxDev,EEZLimit,probe_transform,point_probe_to_local,world,res,
robot,link,CONTROLLER,collider,intermediateConfig):
"""
this is the main function of poking object. - line probe
"""
# reconstruct probepcd.txt
if input('[*]Reconstruct probe pcd?') == 1:
theta_list_num = input('---need theta list number: ')
reconstruct_pcd(config.exp_path+'exp_'+str(config.exp_number)+'/probePcd.txt',
config.exp_path+'exp_'+str(config.exp_number)+'/probePcd_theta.txt',
theta_list_num)
print('---New probe list done')
# Read In the pcd
points, normals, theta_list, theta, pti = load_pcd(config.exp_path+'exp_'+str(config.exp_number)+'/probePcd_theta.txt', pcdtype='xyzrgbntheta')
# control interface
if CONTROLLER == 'physical':
robotControlApi = UR5WithGripperController(host=config.robot_host,gripper=False)
robotControlApi.start()
time.sleep(2)
print '---------------------robot started -----------------------------'
constantVServo(robotControlApi,4,intermediateConfig,dt)#controller format
# set in simul model
robot.setConfig(controller_2_klampt(robot,intermediateConfig))
print '---------------------at home configuration -----------------------------'
if CONTROLLER == 'debugging':
differences=[]
print('[*]Debug: Poking process start')
i = 0 # use this to catch points
pti_ = pti[i]
while(i < len(points)):
robotCurrentConfig=intermediateConfig
goalPosition=deepcopy(points[i])
approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0)) #get unit vector in the direction '- normals'
_pti = pti_
if pti[i] == _pti:
print('point %d, pos: %s, normals: %s, theta: %s, -> %f'%(i,points[i],normals[i],theta_list[i],theta[i]))
## perform IK
local_NY_UnitV = vectorops.unit(back_2_line(approachVector,theta_list[i])) # the probe's line direction
pt1=goalPosition
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) # use 1m in normals direction.
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,use_collision_detect=False,use_ik_detect=True)
differences.append(difference)
print('difference: %f'%difference)
### now start colecting data..
travel = 0.0
stepVector = vectorops.mul(approachVector,moveStep)
while travel<0.0001:
robotCurrentConfig=klampt_2_controller(robot.getConfig())
pt1=vectorops.add(pt1,stepVector)
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,use_collision_detect=False)
travel = travel + moveStep
### move the probe away, note: a bit different to physical mode
robotCurrentConfig=klampt_2_controller(robot.getConfig())
pt1=vectorops.add(points[i],vectorops.mul(approachVector,-0.05)) ## move the probe 5 cm from the object surface
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,use_collision_detect=False)
### move back to intermediate config
robot.setConfig(controller_2_klampt(robot,intermediateConfig))
i = i + 1 # important
else:
pti_ = pti[i]
print('[*]Debug: Poking process done, with max difference:%f'%max(differences))
vis.show()
while vis.shown():
time.sleep(1.0)
elif CONTROLLER == 'physical':
exe_number = input('There are %d poking point, go?'%len(points))
start_i = 0 #72,93,94,97,99,100,101,108,116,125,128,147,148,150,151,152,189,194~197,207~210 !40,37,67,68 -> 111 112 113 120 121 122 201 206
end_i = 1 #len(points)
i = start_i # use this to catch points, set manully! # TODO:
pti_ = pti[i]
probe_list = random.sample(range(282),282) #18,15
finish_list= range(16)+range(17,21)+range(25,44)+range(45,51)+range(52,57)+[58,59,60,63,67,68,71,73,75,76,78]+range(81,96)\
+[97,99,101,103,104,108,110,112,114,115,117,118,120,124,125,128,129,130]+range(132,138)+[141,142,144,147,149,150,151]+range(153,156)\
+[159,160,161,167,168,170,172,175,176,177,178]+range(180,186)\
+[189,192,195,196,199,200,201,203]+range(204,210)+range(211,217)+[219]+range(221,229)+range(230,236)+range(237,241)\
+range(244,250)+[251]+range(254,261)+[262]+range(264,276)+range(277,282)
probe_list = [x for x in probe_list if x not in finish_list] +[227]
probe_list = [95]
for i in probe_list:
print('point %d, pos: %s, normals: %s, theta: %s, -> %f'%(i,points[i],normals[i],theta_list[i],theta[i]))
robotCurrentConfig=robotControlApi.getConfig()
robot.setConfig(controller_2_klampt(robot,robotCurrentConfig))
# calculate start position
goalPosition=deepcopy(points[i])
approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0))
# init record file
forceData=open(config.exp_path+'exp_'+str(config.exp_number)+'/line/force_'+str(i)+'.txt','w')
torqueData=open(config.exp_path+'exp_'+str(config.exp_number)+'/line/torque_'+str(i)+'.txt','w')
travel = -0.025
## perform IK
local_NY_UnitV = vectorops.unit(back_2_line(approachVector,theta_list[i])) # the probe's line direction
#### Make sure no contact, backup 0.01m
pt1=vectorops.add(goalPosition,vectorops.mul(approachVector,travel))
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) # use 1m in normals direction.
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,shortServoTime,dt) #TODO:
time.sleep(0.2)
# Zero the sensor before straight line push, Note that the force is recorded in the global frame..
counter = 0.0
totalF = [0,0,0]
totalTorque = [0,0,0]
startTime=time.time()
while (time.time()-startTime) < 1: # use 1s to cal the Force
totalF = vectorops.add(totalF,robotControlApi.getWrench()[0:3])
totalTorque = vectorops.add(totalTorque,robotControlApi.getWrench()[3:6])
counter = counter + 1.0
time.sleep(dt)
forceBias = vectorops.mul(totalF,1.0/float(counter)) # when probe no touch the obj, F_avr = sum(F)/n
torqueBias = vectorops.mul(totalTorque,1.0/float(counter))
### now start collecting data..
wrench = robotControlApi.getWrench()
Force = vectorops.sub(wrench[0:3],forceBias)
Torque = vectorops.sub(wrench[3:6],torqueBias)
Force_normal = math.fabs(vectorops.dot(Force,approachVector)) #|F||n|cos(theta) = F dot n, set it >= 0
local_Z_UnitV = vectorops.cross(normals[i],local_NY_UnitV)
Torque_normal = vectorops.dot(Torque,local_Z_UnitV) #TODO:
forceHistory = [Force]
force_normalHistory = [Force_normal]
torqueHistory = [Torque]
torque_normalHistory = [Torque_normal]
displacementHistory = [travel]
stepVector = vectorops.mul(approachVector,moveStep)
while Force_normal < forceLimit:
robotCurrentConfig=robotControlApi.getConfig()
robot.setConfig(controller_2_klampt(robot,robotCurrentConfig))
pt1=vectorops.add(pt1,stepVector)
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime,dt,use_const=False)
time.sleep(dt)
Force = vectorops.sub(robotControlApi.getWrench()[0:3],forceBias)
Torque = vectorops.sub(robotControlApi.getWrench()[3:6],torqueBias)
Force_normal = math.fabs(vectorops.dot(Force,approachVector))
local_Z_UnitV = vectorops.cross(normals[i],local_NY_UnitV)
Torque_normal = vectorops.dot(Torque,local_Z_UnitV)
travel = travel + moveStep
forceHistory.append([Force[0],Force[1],Force[2]])
force_normalHistory.append(Force_normal)
torqueHistory.append([Torque[0],Torque[1],Torque[2]])
torque_normalHistory.append(Torque_normal)
displacementHistory.append(travel)
#record all the data in 2 files, one N*2 containts all the force data collected at various locations, another
#file specifies the number of datapoints at each detected point
for (f,fn,d) in zip(forceHistory,force_normalHistory,displacementHistory):
forceData.write(str(f[0])+' '+str(f[1])+' '+str(f[2])+' '+str(fn)+' '+str(d)+'\n')
for (t,tn,d) in zip(torqueHistory,torque_normalHistory,displacementHistory):
torqueData.write(str(t[0])+' '+str(t[1])+' '+str(t[2])+' '+str(tn)+' '+str(d)+'\n')
### move the probe away, sometimes z up 5cm is better than normal direction up 5cm...
pt1=vectorops.add(pt1,[0,0,0.05]) ## move the probe 10 cm up-z-axis, find another point
pt2=vectorops.add(pt2,[0,0,0.05])
pt3=vectorops.add(pt3,[0,0,0.05])
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,shortServoTime-1,dt)
constantVServo(robotControlApi,longServoTime,intermediateConfig,dt)#TODO:
# close record file for point i
forceData.close()
torqueData.close()
print'----------------------- pt '+str(i)+' completed -------------------------------'
'''
while(i < end_i):
robotCurrentConfig=robotControlApi.getConfig()
robot.setConfig(controller_2_klampt(robot,robotCurrentConfig))
# calculate start position
goalPosition=deepcopy(points[i])
approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0))
# init record file
forceData=open(config.exp_path+'exp_'+str(config.exp_number)+'/line/force_'+str(i)+'.txt','w')
torqueData=open(config.exp_path+'exp_'+str(config.exp_number)+'/line/torque_'+str(i)+'.txt','w')
_pti = pti_
if pti[i] == _pti:
print('point %d, pos: %s, normals: %s, theta: %s, -> %f'%(i,points[i],normals[i],theta_list[i],theta[i]))
travel = -0.015
## perform IK
local_NY_UnitV = vectorops.unit(back_2_line(approachVector,theta_list[i])) # the probe's line direction
#### Make sure no contact, backup 0.01m
pt1=vectorops.add(goalPosition,vectorops.mul(approachVector,travel))
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) # use 1m in normals direction.
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime-1,dt) #TODO:
time.sleep(0.2)
# Zero the sensor before straight line push, Note that the force is recorded in the global frame..
counter = 0.0
totalF = [0,0,0]
totalTorque = [0,0,0]
startTime=time.time()
while (time.time()-startTime) < 1: # use 1s to cal the Force
totalF = vectorops.add(totalF,robotControlApi.getWrench()[0:3])
totalTorque = vectorops.add(totalTorque,robotControlApi.getWrench()[3:6])
counter = counter + 1.0
time.sleep(dt)
forceBias = vectorops.mul(totalF,1.0/float(counter)) # when probe no touch the obj, F_avr = sum(F)/n
torqueBias = vectorops.mul(totalTorque,1.0/float(counter))
### now start collecting data..
wrench = robotControlApi.getWrench()
Force = vectorops.sub(wrench[0:3],forceBias)
Torque = vectorops.sub(wrench[3:6],torqueBias)
Force_normal = math.fabs(vectorops.dot(Force,approachVector)) #|F||n|cos(theta) = F dot n, set it >= 0
local_Z_UnitV = vectorops.cross(normals[i],local_NY_UnitV)
Torque_normal = vectorops.dot(Torque,local_Z_UnitV) #TODO:
forceHistory = [Force]
force_normalHistory = [Force_normal]
torqueHistory = [Torque]
torque_normalHistory = [Torque_normal]
displacementHistory = [travel]
stepVector = vectorops.mul(approachVector,moveStep)
while Force_normal < forceLimit:
robotCurrentConfig=robotControlApi.getConfig()
robot.setConfig(controller_2_klampt(robot,robotCurrentConfig))
pt1=vectorops.add(pt1,stepVector)
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime,dt,use_const=False)
time.sleep(dt)
Force = vectorops.sub(robotControlApi.getWrench()[0:3],forceBias)
Torque = vectorops.sub(robotControlApi.getWrench()[3:6],torqueBias)
Force_normal = math.fabs(vectorops.dot(Force,approachVector))
local_Z_UnitV = vectorops.cross(normals[i],local_NY_UnitV)
Torque_normal = vectorops.dot(Torque,local_Z_UnitV)
travel = travel + moveStep
forceHistory.append([Force[0],Force[1],Force[2]])
force_normalHistory.append(Force_normal)
torqueHistory.append([Torque[0],Torque[1],Torque[2]])
torque_normalHistory.append(Torque_normal)
displacementHistory.append(travel)
#record all the data in 2 files, one N*2 containts all the force data collected at various locations, another
#file specifies the number of datapoints at each detected point
for (f,fn,d) in zip(forceHistory,force_normalHistory,displacementHistory):
forceData.write(str(f[0])+' '+str(f[1])+' '+str(f[2])+' '+str(fn)+' '+str(d)+'\n')
for (t,tn,d) in zip(torqueHistory,torque_normalHistory,displacementHistory):
torqueData.write(str(t[0])+' '+str(t[1])+' '+str(t[2])+' '+str(tn)+' '+str(d)+'\n')
### move the probe away, sometimes z up 5cm is better than normal direction up 5cm...
pt1=vectorops.add(pt1,[0,0,0.05]) ## move the probe 10 cm up-z-axis, find another point
pt2=vectorops.add(pt2,[0,0,0.05])
pt3=vectorops.add(pt3,[0,0,0.05])
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,shortServoTime-0.5,dt)
#constantVServo(robotControlApi,longServoTime,intermediateConfig,dt)#TODO:
i = i + 1
# close record file for point i
forceData.close()
torqueData.close()
print'----------------------- pt '+str(i)+' completed -------------------------------'
else:
# up 10cm is faster but not good.
# since the points are close, no need to go back home
constantVServo(robotControlApi,longServoTime,intermediateConfig,dt)
pti_ = pti[i]
'''
#### move back to intermediate config
constantVServo(robotControlApi,shortServoTime,intermediateConfig,dt)
# finish all points
robotControlApi.stop()
def cal_curvature(pcdSegmented):
NofPts = len(pcdSegmented.points)
NofNN = 30
searchR = 0.01 #the points are 2mm apart...
pcdTree = KDTreeFlann(pcdSegmented)
curvatures = []
print 'calculating curvatures......'
for i in range(len(pcdSegmented.points)):
#for i in [0]:
[k, idx, _] = pcdTree.search_knn_vector_3d(pcdSegmented.points[i],
NofNN + 1)
#[k,idx,_] = pcdTree.search_radius_vector_3d(pcdSegmented.points[i],searchR)
#k is the total Number, while idx is the list of indices
#for debugging
#for ii in idx:
# pcdSegmente.colors[ii] = [0,1,0]
#draw_geometries([pcdSegmented])
#if k < 3.5:
# [k,idx,_] = pcdTree.search_knn_vector_3d(pcdSegmented.points[i],NofNN+1)
nearbyPts = np.asarray(pcdSegmented.points)[idx[1:], :]
nearbyPtsNormal = np.asarray(pcdSegmented.normals)[idx[1:], :]
normalCurvatures = []
## Define the local coordinates
N = vectorops.unit(pcdSegmented.normals[i]) # also the "z"
Y = vectorops.cross(N, [1, 0, 0]) # pq = vectorops.(q,p)
X = vectorops.cross(Y, N)
MMatrix = []
for j in range(k - 1):
#estimate the point normal, in Local corrdinate
qGlobal = vectorops.sub(nearbyPts[j], pcdSegmented.points[i])
qLocal = [
vectorops.dot(qGlobal, X),
vectorops.dot(qGlobal, Y),
vectorops.dot(qGlobal, N)
]
MGlobal = nearbyPtsNormal[j]
MLocal = [
vectorops.dot(MGlobal, X),
vectorops.dot(MGlobal, Y),
vectorops.dot(MGlobal, N)
]
[x_i, y_i, z_i] = qLocal
[n_xi, n_yi, n_zi] = MLocal
#print x_i,n_xi,y_i,n_yi
n_xy = (x_i * n_xi + y_i * n_yi) / math.sqrt(x_i * x_i + y_i * y_i)
#print n_xy
k_i_n = -(n_xy) / (math.sqrt(n_xy * n_xy + n_zi * n_zi) +
math.sqrt(x_i * x_i + y_i * y_i))
normalCurvatures.append(k_i_n)
## calculate the M matrix
pQ = [qLocal[0], qLocal[1], 0]
angle = np.arccos(
np.dot(X, pQ) / (np.linalg.norm(X) * np.linalg.norm(pQ)))
row = [
math.cos(angle) * math.cos(angle),
2 * math.cos(angle) * math.sin(angle),
math.sin(angle) * math.sin(angle)
]
MMatrix.append(row)
#perform least squres fitting
tmp = np.dot(np.transpose(MMatrix), MMatrix)
tmp2 = np.dot(np.linalg.inv(tmp), np.transpose(MMatrix))
x = tmp2.dot(normalCurvatures)
eigenValues, v = np.linalg.eig([[x[0], x[1]], [x[1], x[2]]])
curvatures.append(vectorops.norm_L1(eigenValues))
#print vectorops.norm_L1(eigenValues)
print 'progress', float(i) / float(NofPts)
print "max and min of curvatures: ", np.max(curvatures), np.min(curvatures)
return curvatures
def run_poking_point_probe(config,tableHeight,probeLength,forceLimit,dt,moveStep,shortServoTime,longServoTime,
IKErrorTolerence,maxDev,EEZLimit,probe_transform,point_probe_to_local,world,res,
robot,link,CONTROLLER,collider,intermediateConfig):
"""
this is the main function of poking object. - point probe
"""
# Read In the pcd
points, normals = load_pcd(config.exp_path+'exp_'+str(config.exp_number)+'/probePcd.txt')
# control interface
if CONTROLLER == 'physical':
robotControlApi = UR5WithGripperController(host=config.robot_host,gripper=False)
robotControlApi.start()
time.sleep(2)
print '---------------------robot started -----------------------------'
constantVServo(robotControlApi,4,intermediateConfig,dt)#controller format
# in simulation ,set
robot.setConfig(controller_2_klampt(robot,intermediateConfig))
print '---------------------at home configuration -----------------------------'
if CONTROLLER == 'debugging':
differences=[]
print('[*]Debug: Poking process start!')
for i in range(len(points)):
print('point %d, pos: %s, normals: %s'%(i,points[i],normals[i]))
goalPosition=deepcopy(points[i])
approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0)) #get unit vector in the direction '- normals'
## perform IK
local_NY_UnitV=vectorops.unit(vectorops.cross([0,1,0],approachVector))
pt1=goalPosition
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) # use 1m in normals direction.
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],maxDev,
IKErrorTolerence,EEZLimit,collider,use_collision_detect=True)
differences.append(difference)
print('difference: %f'%difference)
### now start colecting data..
travel = 0.0
stepVector = vectorops.mul(approachVector,moveStep)
while travel<0.0001: #just try 0.1mm?
pt1=vectorops.add(pt1,stepVector)
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],maxDev,
IKErrorTolerence,EEZLimit,collider,use_const=False)
travel = travel + moveStep
### move the probe away, note: a bit different to physical mode
pt1=vectorops.add(points[i],vectorops.mul(approachVector,-0.05)) ## move the probe 5 cm from the object surface
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],maxDev,
IKErrorTolerence,EEZLimit,collider)
### move back to intermediate config
robot.setConfig(controller_2_klampt(robot,intermediateConfig))
print('[*]Debug: Poking process done, with max difference:%f'%max(differences))
vis.show()
while vis.shown():
time.sleep(1.0)
elif CONTROLLER == 'physical':
input('There are %d poking point, go?'%len(points))
point_list = range(112,116) # !delete 85, 90
#point_list = random.sample(range(97),97)
#point_list = [40,37,67,68]
for i in point_list:
print('point %d, pos: %s, normals: %s'%(i,points[i],normals[i]))
travel = -0.01
#init record file
forceData=open(config.exp_path+'exp_'+str(config.exp_number)+'/point/force_'+str(i)+'.txt','w')
robotCurrentConfig=robotControlApi.getConfig()
robot.setConfig(controller_2_klampt(robot,robotCurrentConfig))
#calculate start position
goalPosition=deepcopy(points[i])
approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0))
#### Make sure no contact, backup 0.01m
local_NY_UnitV=vectorops.unit(vectorops.cross([0,1,0],approachVector))
pt1=vectorops.add(goalPosition,vectorops.mul(approachVector,travel))
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime-1,dt) #TODO:
time.sleep(0.2)
# Zero the sensor before straight line push, Note that the force is recorded in the global frame..
counter = 0.0
totalF = [0,0,0]
startTime=time.time()
while (time.time()-startTime) < 1: # use 1s to cal the Force
totalF = vectorops.add(totalF,robotControlApi.getWrench()[0:3])
counter = counter + 1.0
time.sleep(dt)
forceBias = vectorops.mul(totalF,1.0/float(counter)) # when probe no touch the obj, F_avr = sum(F)/n
### now start collecting data..
wrench = robotControlApi.getWrench()
Force = vectorops.sub(wrench[0:3],forceBias)
Force_normal = math.fabs(vectorops.dot(Force,approachVector)) #|F||n|cos(theta) = F dot n, set it >= 0
forceHistory = [Force]
force_normalHistory = [Force_normal]
displacementHistory = [travel]
stepVector = vectorops.mul(approachVector,moveStep)
while Force_normal < forceLimit:
robotCurrentConfig=robotControlApi.getConfig()
robot.setConfig(controller_2_klampt(robot,robotCurrentConfig))
pt1=vectorops.add(pt1,stepVector)
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime,dt,use_const=False)
time.sleep(dt)
Force = vectorops.sub(robotControlApi.getWrench()[0:3],forceBias)
Force_normal = math.fabs(vectorops.dot(Force,approachVector))
travel = travel + moveStep
forceHistory.append([Force[0],Force[1],Force[2]])
force_normalHistory.append(Force_normal)
displacementHistory.append(travel)
#record all the data in 2 files, one N*2 containts all the force data collected at various locations, another
#file specifies the number of datapoints at each detected point
for (f,fn,d) in zip(forceHistory,force_normalHistory,displacementHistory):
forceData.write(str(f[0])+' '+str(f[1])+' '+str(f[2])+' '+str(fn)+' '+str(d)+'\n')
forceData.close()
### move the probe away
robotCurrentConfig=robotControlApi.getConfig()
robot.setConfig(controller_2_klampt(robot,robotCurrentConfig))
pt1=vectorops.add(points[i],vectorops.mul(approachVector,-0.10)) ## move the probe 8 cm from the object surface
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,shortServoTime,dt)
#constantVServo(robotControlApi,longServoTime,intermediateConfig,dt)
print'----------------------- pt '+str(i)+' completed -------------------------------'
#### move back to intermediate config
constantVServo(robotControlApi,shortServoTime,intermediateConfig,dt)
robotControlApi.stop()
def run_poking_ellipse_probe(config,tableHeight,probeLength,forceLimit,dt,moveStep,shortServoTime,longServoTime,
IKErrorTolerence,maxDev,EEZLimit,probe_transform,point_probe_to_local,world,res,
robot,link,CONTROLLER,collider,intermediateConfig):
"""
this is the main function of poking object. - point probe
"""
########################## Read In the pcd ######################################
points, normals = load_pcd(config.exp_path+'exp_'+str(config.exp_number)+'/probePcd.txt')
# control interface
if CONTROLLER == 'physical':
robotControlApi = UR5WithGripperController(host=config.robot_host,gripper=False)
robotControlApi.start()
time.sleep(2)
print '---------------------robot started -----------------------------'
## Record some home configuration
intermediateConfig = config.intermediateConfig
intermediateConfig = intermediateConfig
if CONTROLLER == "physical":
constantVServo(robotControlApi,4,intermediateConfig,dt)#controller format
robot.setConfig(controller_2_klampt(robot,intermediateConfig))
print '---------------------at home configuration -----------------------------'
if CONTROLLER == 'debugging':
differences=[]
print('[*]Debug: Poking process start!')
for i in range(len(points)):
print('point %d, pos: %s, normals: %s'%(i,points[i],normals[i]))
#robotCurrentConfig=intermediateConfig # TODO: compare to the intermediateConfig, I comment it
goalPosition=deepcopy(points[i])
approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0)) #get unit vector in the direction '- normals'
## perform IK
local_NY_UnitV=vectorops.unit(vectorops.cross([0,1,0],approachVector))
pt1=goalPosition
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength)) # use 1m in normals direction.
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],maxDev,
IKErrorTolerence,EEZLimit,collider,use_collision_detect=True,use_ik_detect=True)
differences.append(difference)
print('difference: %f'%difference)
### now start colecting data..
travel = 0.0
stepVector = vectorops.mul(approachVector,moveStep)
while travel<0.0001: #just try 0.1mm?
pt1=vectorops.add(pt1,stepVector)
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],maxDev,
IKErrorTolerence,EEZLimit,collider,use_const=False)
travel = travel + moveStep
### move the probe away, note: a bit different to physical mode
pt1=vectorops.add(points[i],vectorops.mul(approachVector,-0.05)) ## move the probe 5 cm from the object surface
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],maxDev,
IKErrorTolerence,EEZLimit,collider)
### move back to intermediate config
robot.setConfig(controller_2_klampt(robot,intermediateConfig))
print('[*]Debug: Poking process done, with max difference:%f'%max(differences))
vis.show()
while vis.shown():
time.sleep(1.0)
elif CONTROLLER == 'physical':
######################################## Ready to Take Measurements ################################################
input('[!]Warning: There are %d poking point, Robot act!:'%len(points))
point_list = range(65,120) #64
#point_list = random.sample(range(0,94),94)
#finish_list = [0,1,4,9,10,11,12,13,14,15,17,18,19,20,25,26,28,30,33,34,35,37,42,43,44,46,47,50,51,53,54,57,58,59,64,69,72,73,74,75,76,77,78,79,81,83,86,95]
#point_list = [x for x in point_list if x not in finish_list]
point_list = [64]
for i in point_list:
print('point %d, pos: %s, normals: %s'%(i,points[i],normals[i]))
#init record file
forceData=open(config.exp_path+'exp_'+str(config.exp_number)+'/ellipse/force_'+str(i)+'.txt','w')
torqueData=open(config.exp_path+'exp_'+str(config.exp_number)+'/ellipse/torque_'+str(i)+'.txt','w')
#init the backforward distance
travel = -0.018
robotCurrentConfig=robotControlApi.getConfig()
robot.setConfig(controller_2_klampt(robot,robotCurrentConfig))
#calculate start position
goalPosition=deepcopy(points[i])
approachVector=vectorops.unit(vectorops.mul(normals[i],-1.0))
#### Make sure no contact, backup 0.01m
local_NY_UnitV=vectorops.unit(vectorops.cross([0,1,0],approachVector))
pt1=vectorops.add(goalPosition,vectorops.mul(approachVector,travel))
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime-1,dt,
use_ik_detect=False,use_collision_detect=False)
time.sleep(0.2)
## Zero the sensor before straight line push
#
# Note that the force is recorded in the global frame..
# And the global frame has x and y axis flipped w.r.t the URDF....
counter = 0.0
totalF = [0,0,0]
totalTorque = [0,0,0]
startTime=time.time()
while (time.time()-startTime) < 1: # use 1s to cal the Force
totalF = vectorops.add(totalF,robotControlApi.getWrench()[0:3])
totalTorque = vectorops.add(totalTorque,robotControlApi.getWrench()[3:6])
counter = counter + 1.0
time.sleep(dt)
forceBias = vectorops.mul(totalF,1.0/float(counter)) # when probe no touch the obj, F_avr = sum(F)/n
torqueBias = vectorops.mul(totalTorque,1.0/float(counter))
### now start collecting data..
# Force direction x, y inverse, refer to correct force.py
wrench = robotControlApi.getWrench()
Force = fix_direction(vectorops.sub(wrench[0:3],forceBias))
Force_normal = math.fabs(vectorops.dot(Force,approachVector)) #|F||n|cos(theta) = F dot n, set it >= 0
Torque = vectorops.sub(wrench[3:6],torqueBias)
Torque = fix_direction(Torque)
forceHistory = [Force]
torqueHistory = [Torque]
force_normalHistory = [Force_normal]
displacementHistory = [travel]
stepVector = vectorops.mul(approachVector,moveStep)
while Force_normal < forceLimit:
robotCurrentConfig=robotControlApi.getConfig()
robot.setConfig(controller_2_klampt(robot,robotCurrentConfig))
pt1=vectorops.add(pt1,stepVector)
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,longServoTime,dt,
use_const=False,use_ik_detect=False)
time.sleep(dt)
Force = fix_direction(vectorops.sub(robotControlApi.getWrench()[0:3],forceBias))
Force_normal = math.fabs(vectorops.dot(Force,approachVector))
Torque = vectorops.sub(robotControlApi.getWrench()[3:6],torqueBias)
travel = travel + moveStep
forceHistory.append([Force[0],Force[1],Force[2]])
torqueHistory.append([Torque[0],Torque[1],Torque[2]])
force_normalHistory.append(Force_normal)
displacementHistory.append(travel)
#record all the data in 2 files, one N*2 containts all the force data collected at various locations, another
#file specifies the number of datapoints at each detected point
for (f,fn,d) in zip(forceHistory,force_normalHistory,displacementHistory):
forceData.write(str(f[0])+' '+str(f[1])+' '+str(f[2])+' '+str(fn)+' '+str(d)+'\n')
for (t,d) in zip(torqueHistory,displacementHistory):
torqueData.write(str(t[0])+' '+str(t[1])+' '+str(t[2])+' '+str(d)+'\n')
forceData.close()
torqueData.close()
### move the probe away
robotCurrentConfig=robotControlApi.getConfig()
robot.setConfig(controller_2_klampt(robot,robotCurrentConfig))
pt1=vectorops.add(points[i],vectorops.mul(approachVector,-0.10)) ## move the probe 5 cm from the object surface
pt2=vectorops.add(pt1,vectorops.mul(approachVector,1.0-probeLength))
pt3=vectorops.add(pt1,local_NY_UnitV)
[robot,difference] = robot_move(CONTROLLER,world,robot,link,point_probe_to_local,[pt1,pt2,pt3],
maxDev,IKErrorTolerence,EEZLimit,collider,robotControlApi,shortServoTime,dt,
use_ik_detect=False)
#constantVServo(robotControlApi,longServoTime,intermediateConfig,dt)
print'----------------------- pt '+str(i)+' completed -------------------------------'
#### move back to intermediate config
constantVServo(robotControlApi,shortServoTime,intermediateConfig,dt)
robotControlApi.stop()
def load_data(point_path, force_path, probe_type='point', datatype='1'):
points=[]
colors=[]
normals=[]
curvatures=[]
theta = []
dataFile=open(point_path,'r')
for line in dataFile:
line=line.rstrip()
l=[num for num in line.split(' ')]
l2=[float(num) for num in l]
points.append(l2[0:3])
colors.append(l2[3:6])
normals.append(l2[6:9])
curvatures.append(l2[9])
#normalize
tmp_theta = vectorops.unit(l2[10:13])
theta.append(tmp_theta)
dataFile.close()
# normalize, note colors and normals is 0~1
points = np.array(points)
colors = np.array(colors)
normals = np.array(normals)
curvatures = np.array(curvatures)
max_range = max([ (np.max(points[:,0])-np.min(points[:,0])) , (np.max(points[:,1])-np.min(points[:,1])) , (np.max(points[:,2])-np.min(points[:,2])) ])
for i in range(3):
points[:,i] = (points[:,i]-np.min(points[:,i]))/max_range
num_point = len(points)
print('[*]load %d points, and normalized'%num_point)
'''
X = np.array([[]])
Y = np.array([[]])
insert_i = 0
'''
X=[]
Y=[]
for i in range(num_point):
force_path = './'+probe_type+'/force_'+str(i)+'.txt'
force=[]
force_normal=[]
displacement=[]
dataFile=open(force_path,'r')
for line in dataFile:
line=line.rstrip()
l=[num for num in line.split(' ')]
l2=[float(num) for num in l]
force.append(l2[0:3])
force_normal.append(l2[3])
displacement.append(l2[4])
dataFile.close()
# clean
#TODO:
# final
if probe_type == 'point':
num_dis = len(displacement)
#print('---load %d displacement'%num_dis)
displacement = np.resize(np.array(displacement),(num_dis,1))
X_i = np.hstack((np.tile(points[i],(num_dis,1)), np.tile(normals[i],(num_dis,1)), np.tile(theta[i],(num_dis,1)) displacement))
Y_i = np.array(force_normal,ndmin=2).T
'''
if insert_i == 0:
X=X_i
Y=Y_i
else:
X = np.vstack((X,X_i))
Y = np.vstack((Y,Y_i))
insert_i = insert_i + 1
'''
X.append(X_i)
Y.append(Y_i)
return X,Y
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 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 loop_through_sensors(
world=world,
sensor=sensor,
world_camera_viewpoints=world_camera_viewpoints,
index=index,
counters=counters):
viewno = counters[0]
variation = counters[1]
total_count = counters[2]
print("Generating data for sensor view", viewno, "variation",
variation)
sensor_xform = world_camera_viewpoints[viewno]
#TODO: problem 1.B: perturb camera and change colors
# Generate random axis, random angle, rotate about angle for orientation perturbation
# Generate random axis, random dist, translate distance over axis for position perturbation
rand_axis = np.random.rand(3)
rand_axis = vectorops.unit(rand_axis)
multiplier = np.random.choice([True, False], 1)
rand_angle = (np.random.rand(1)[0] * 10 +
10) * (-1 if multiplier else 1)
# print(rand_angle)
R = so3.from_axis_angle((rand_axis, np.radians(rand_angle)))
rand_axis = vectorops.unit(np.random.random(3))
rand_dist = np.random.random(1)[0] * .10 + .10
# print(rand_dist)
t = vectorops.mul(rand_axis, rand_dist)
sensor_xform = se3.mul((R, t), sensor_xform)
sensing.set_sensor_xform(sensor, sensor_xform)
rgb_filename = "image_dataset/color_%04d_var%04d.png" % (
total_count, variation)
depth_filename = "image_dataset/depth_%04d_var%04d.png" % (
total_count, variation)
grasp_filename = "image_dataset/grasp_%04d_var%04d.png" % (
total_count, variation)
#Generate sensor images
sensor.kinematicReset()
sensor.kinematicSimulate(world, 0.01)
print("Done with kinematic simulate")
rgb, depth = sensing.camera_to_images(sensor)
print("Saving to", rgb_filename, depth_filename)
Image.fromarray(rgb).save(rgb_filename)
depth_scale = 8000
depth_quantized = (depth * depth_scale).astype(np.uint32)
Image.fromarray(depth_quantized).save(depth_filename)
with open("image_dataset/metadata.csv", 'a') as f:
line = "{},{},{},{},{},{},{}\n".format(
index, viewno, loader.write(sensor_xform,
'RigidTransform'),
os.path.basename(rgb_filename),
os.path.basename(depth_filename),
os.path.basename(grasp_filename), variation)
f.write(line)
#calculate grasp map and save it
grasp_map = make_grasp_map(world, total_count)
grasp_map_quantized = (grasp_map * 255).astype(np.uint8)
channels = ['score', 'opening', 'axis_heading', 'axis_elevation']
for i, c in enumerate(channels):
base, ext = os.path.splitext(grasp_filename)
fn = base + '_' + c + ext
Image.fromarray(grasp_map_quantized[:, :, i]).save(fn)
#loop through variations and views
counters[1] += 1
if counters[1] >= max_variations:
counters[1] = 0
counters[0] += 1
if counters[0] >= len(world_camera_viewpoints):
vis.show(False)
counters[2] += 1
def sphere_staggered_grid(N):
"""Returns a nx.Graph describing a staggered grid on S2 with resolution N.
The resulting graph has ~12N^2 nodes
The node attribute 'params' gives the so3 element.
"""
import itertools
assert N >= 1
G = nx.Graph()
namemap = dict()
for ax in range(3):
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)
idx = [i, j]
idx = idx[:ax] + [N] + idx[ax:]
idx = tuple(idx)
if idx in namemap:
pass
else:
namemap[idx] = idx
x = [u, v]
x = x[:ax] + [1.0] + x[ax:]
x = vectorops.unit(x)
G.add_node(idx, params=x)
if i < N and j < N:
#add staggered point
sidx = [i + 0.5, j + 0.5]
sidx = sidx[:ax] + [N] + sidx[ax:]
sidx = tuple(sidx)
namemap[sidx] = sidx
x = [u2, v2]
x = x[:ax] + [1.0] + x[ax:]
x = vectorops.unit(x)
G.add_node(sidx, params=x)
idx = [i, j]
idx = idx[:ax] + [0] + idx[ax:]
idx = tuple(idx)
if idx in namemap:
pass
else:
namemap[idx] = idx
x = [u, v]
x = x[:ax] + [-1.0] + x[ax:]
x = vectorops.unit(x)
G.add_node(idx, params=x)
if i < N and j < N:
#add staggered point
sidx = [i + 0.5, j + 0.5]
sidx = sidx[:ax] + [0] + sidx[ax:]
sidx = tuple(sidx)
namemap[sidx] = sidx
x = [u2, v2]
x = x[:ax] + [-1.0] + x[ax:]
x = vectorops.unit(x)
G.add_node(sidx, params=x)
for ax in range(3):
for i in xrange(N + 1):
for j in xrange(N + 1):
baseidx = [i, j]
idx = baseidx[:ax] + [N] + baseidx[ax:]
idx = tuple(idx)
for n in range(2):
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])
idx = baseidx[:ax] + [0] + baseidx[ax:]
idx = tuple(idx)
for n in range(2):
nidx = baseidx[:]
nidx[n] += 1
if nidx[n] > N: continue
nidx = nidx[:ax] + [0] + 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]
if baseidx[0] > N or baseidx[1] > N:
continue
idx = baseidx[:ax] + [N] + baseidx[ax:]
idx = tuple(idx)
for ofs in itertools.product(*[(-0.5, 0.5)] * 2):
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])
idx = baseidx[:ax] + [0] + baseidx[ax:]
idx = tuple(idx)
for ofs in itertools.product(*[(-0.5, 0.5)] * 2):
nidx = vectorops.add(baseidx, ofs)
nidx = [int(x) for x in nidx]
nidx = nidx[:ax] + [0] + nidx[ax:]
nidx = tuple(nidx)
#print "Stagger-grid edge",idx,nidx
G.add_edge(namemap[idx], namemap[nidx])
return G
