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 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 add_multiple_lights(properties,object,dist,numLight,gravity=[0,0,-9.81],tgt=None,color=[1.,1.,1.], \
spotlight=False,radius=15.,falloff=20.,tightness=10., \
area=0.,sample=9,adaptive=True,jitter=True):
"""This is a convenient function that calls add_light multiple times"""
#normalize gravity
g = op.mul(gravity, -1 / op.norm(gravity))
#compute frame
gabs = [abs(gi) for gi in g]
id = gabs.index(min(gabs))
t0 = [1. if i == id else 0. for i in range(3)]
t1 = op.cross(t0, g)
t1 = op.mul(t1, 1 / op.norm(t1))
t0 = op.cross(t1, g)
#find highest direction
bb = compute_bb(object)
ctr = op.mul(op.add(bb[0], bb[1]), 0.5)
distg = sum([abs((bb[1][i] - bb[0][i]) / 2 * g[i]) for i in range(3)])
#add each light
for i in range(numLight):
angle = math.pi * 2 * i / numLight
d0 = op.mul(g, distg)
d1 = op.mul(t0, math.sin(angle) * dist)
d2 = op.mul(t1, math.cos(angle) * dist)
add_light(properties, op.add(d0, op.add(d1,
d2)), ctr, color, spotlight,
radius, falloff, tightness, area, sample, adaptive, jitter)
def solve_3R_forward_kinematics(q1,q2,q3,L1=1,L2=1,L3=1):
"""Returns a list of (x,y,theta) triples for each link
for a planar, 3R manipulator with link lengths L1, L2, L3.
It also returns the end effector transform."""
T1 = (0,0,q1)
dx1 = (L1*math.cos(T1[2]),L1*math.sin(T1[2]))
T2 = vectorops.add((T1[0],T1[1]),dx1)+[T1[2]+q2]
dx2 = (L2*math.cos(T2[2]),L2*math.sin(T2[2]))
T3 = vectorops.add((T2[0],T2[1]),dx2)+[T2[2]+q3]
dx3 = (L3*math.cos(T3[2]),L2*math.sin(T3[2]))
T4 = vectorops.add((T3[0],T3[1]),dx3)+[T3[2]]
return [T1,T2,T3,T4]
def drawConfig(self, q=None):
if self.drawJointLimitColors:
testq = q
if q is None:
testq = self.robot.getConfig()
oldColors = []
for i in xrange(self.robot.numLinks()):
oldColors.append(self.robot.link(i).appearance().getColor())
if self.robot.getJointType(i) == 'normal':
a, b = self.jointLimits[0][i], self.jointLimits[1][i]
u = 1.0
u = min(1, (testq[i] - a) / (self.jointLimitColoringRatio *
(b - a)))
u = min(1, (b - testq[i]) / (self.jointLimitColoringRatio *
(b - a)))
if u != 1:
u = max(u, 0.0)
c = vectorops.add(
vectorops.mul(oldColors[i], u),
vectorops.mul(self.jointLimitColor, 1 - u))
self.robot.link(i).appearance().setColor(*c)
if q is not None:
self.robot.setConfig(q)
self.robot.drawGL()
if self.drawJointLimitColors:
for i in xrange(self.robot.numLinks()):
self.robot.link(i).appearance().setColor(*oldColors[i])
def opt_error_fun(est_input, *args):
"""
:param est_input: {numpy.array} -- the initial guess of the transformation of Toct and Tneedle
:param args: {tuple} -- the set of tranfermation of Tlink and ICP transformation matrix
:return: error: {float} -- the error between the true transformation and estimated transformation
"""
Roct = so3.from_rpy(est_input[0:3])
toct = est_input[3:6]
Rn = so3.from_rpy(est_input[6:9])
tn = est_input[9:12]
Tlink_set = args[0]
Tneedle2oct_icp = args[1]
fun_list = np.array([])
for i in range(len(Tlink_set)):
fun_list = np.append(
fun_list,
np.multiply(
so3.error(so3.inv(Tneedle2oct_icp[i][0]),
so3.mul(so3.mul(so3.inv(Roct), Tlink_set[i][0]),
Rn)), 1))
fun_list = np.append(
fun_list,
np.multiply(
vectorops.sub(
vectorops.sub([0., 0., 0.],
so3.apply(so3.inv(Tneedle2oct_icp[i][0]),
Tneedle2oct_icp[i][1])),
so3.apply(
so3.inv(Roct),
vectorops.add(vectorops.sub(Tlink_set[i][1], toct),
so3.apply(Tlink_set[i][0], tn)))), 1000))
return fun_list
def get_camera_pos(self):
cam = self.view.camera
z = math.sin(-cam.rot[1])
x = math.sin(cam.rot[2]) * math.cos(cam.rot[1])
y = math.cos(cam.rot[2]) * math.cos(cam.rot[1])
pos = [x, y, z]
return op.add(cam.tgt, op.mul(pos, cam.dist))
def feasible(self, q):
"""TODO: Implement this feasibility test. It is used by the motion planner to
determine whether the robot at configuration q is feasible."""
robotRadius = self.robot.radius
# bounds test
if not CSpace.feasible(self, q): return False
for o in self.obstacles:
if o.distance(q) <= robotRadius: return False
# bound the robot in a square defined by robot's radius
bottomLeft = vectorops.add(q, (-robotRadius, -robotRadius))
topRight = vectorops.add(q, (robotRadius, robotRadius))
# make sure you can put the square in the config space
# if topLeft is not feasible bottomLeft or topRight will be infeasible
# if bottomRight is not feasible bottomLeft or topRight will be infeasible
boundingSquareFeasible = CSpace.feasible(
self, bottomLeft) and CSpace.feasible(self, topRight)
return boundingSquareFeasible
def make_object_arrangement(world,
container,
objects,
container_wall_thickness=0.01,
max_iterations=100,
remove_failures=False):
"""For a given container and a list of objects in the world, places the objects inside the container with randomized x-y locations
and z orientations so that they are initially collision free and on the bottom of the container.
Args:
world (WorldModel): the world containing the objects and obstacles
container: the container RigidObject / Terrain in world into which
objects should be spawned. Assumed axis-aligned.
objects (list of RigidObject): a list of RigidObjects in the world,
at arbitrary locations. They are placed in order.
container_wall_thickness (float, optional): a margin subtracted from
the container's outer dimensions into which the objects are spawned.
max_iterations (int, optional): the maximum number of iterations used
for sampling object initial poses
remove_failures (bool): if True, then instead of returning None on
failure, the objects that fail placement are removed from the world.
Returns:
(WorldModel): if successful, the positions of objects in world are
modified and world is returned. On failure, None is returned.
Note:
Since world is modified in-place, if you wish to make multiple worlds with
piles of the same objects, you should use world.copy() to store the
configuration of the objects. You may also wish to randomize the object
ordering using random.shuffle(objects) between instances.
"""
container_outer_bb = _get_bound(container)
container_inner_bb = (vectorops.add(container_outer_bb[0],
[container_wall_thickness] * 3),
vectorops.sub(container_outer_bb[1],
[container_wall_thickness] * 3))
collision_margin = 0.0025
for object in objects:
#make sure the bottom of the object touches the bottom of the container
obb = _get_bound(object)
zmin = obb[0][2]
R, t = object.getTransform()
t[2] = container_inner_bb[2] - zmin + collision_margin
object.setTransform(R, t)
failures = xy_jiggle(world, rigid_objects, [container],
container_inner_bb[0], container_inner_bb[1],
max_iterations)
if len(failures) > 0:
if remove_failures:
removeIDs = [objects[i].index for i in failures]
for i in sorted(removeIDs)[::-1]:
world.remove(world.rigidObject(i))
else:
return None
return world
def control_loop(self):
#Calculate the desired velocity for each robot by adding up all
#commands
rvels = [[0] * 3 for r in range(self.world.numRobots())]
for (c, (r, v)) in self.current_velocities.iteritems():
rvels[r] = vectorops.add(rvels[r], v)
#send to the robot(s)
for r in range(self.world.numRobots()):
self.spheros[r].process({'twist': rvels[r]}, self.dt)
return
def configSolver(cur_target):
global robot
end_link = robot.link(robot.numLinks() - 1)
ee_local_axis = end_link.getAxis()
ee_local_position = (0, 0, 0)
cur_target_axis = (0, 0, -1)
goal = ik.objective(
end_link,
local=[
ee_local_position,
vectorops.add(ee_local_position, ee_local_axis)
], # match end points
world=[cur_target,
vectorops.add(cur_target, cur_target_axis)])
ik.solve_global(goal)
# ### Test Print
# print('Local Coord: ', [ee_local_position, vectorops.add(ee_local_position, (0,0,-0.01))])
# print('Global Coord: ', [cur_target, vectorops.add(cur_target, (0,0,0.01))])
# ###
return robot.getConfig()
def keyboardfunc(self, c, x, y):
#Put your keyboard handler here
#the current example prints the config when [space] is pressed
if c == 'h' or c == '?':
print("Controls:")
print("- [space]: print the widget's sub-robot configuration")
print("- r: randomize the sub-robot configuration")
print("- p: plan to the widget's sub-robot configuration")
print("- i: test the IK functions")
elif c == ' ':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
print("Config:", subconfig)
self.subrobots[0].setConfig(subconfig)
elif c == 'r':
self.subrobots[0].randomizeConfig()
self.robotWidget.set(self.robot.getConfig())
elif c == 'p':
config = self.robotWidget.get()
subconfig = self.subrobots[0].fromfull(config)
self.subrobots[0].setConfig(self.subrobots[0].fromfull(
self.startConfig))
settings = {
'type': "sbl",
'perturbationRadius': 0.5,
'bidirectional': 1,
'shortcut': 1,
'restart': 1,
'restartTermCond': "{foundSolution:1,maxIters:1000}"
}
plan = robotplanning.planToConfig(self.world,
self.subrobots[0],
subconfig,
movingSubset='all',
**settings)
plan.planMore(1000)
print(plan.getPath())
elif c == 'i':
link = self.subrobots[0].link(self.subrobots[0].numLinks() - 1)
print("IK solve for ee to go 10cm upward...")
obj = ik.objective(link,
local=[0, 0, 0],
world=vectorops.add(
link.getWorldPosition([0, 0, 0]),
[0, 0, 0.1]))
solver = ik.solver(obj)
res = solver.solve()
print(" result", res)
print(" residual", solver.getResidual())
print(self.robotWidget.set(self.robot.getConfig()))
else:
GLWidgetPlugin.keyboardfunc(self, c, x, y)
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 planTransfer(world, objectIndex, hand, shift):
"""Plan a transfer path for the robot given in world, which is currently
holding the object indexed by objectIndex in the hand hand.
The desired motion should translate the object by shift without rotating
the object.
"""
globals = Globals(world)
obj = world.rigidObject(objectIndex)
cspace = TransferCSpace(globals, hand, obj)
robot = world.robot(0)
qmin, qmax = robot.getJointLimits()
#get the start config
q0 = robot.getConfig()
q0arm = [q0[i] for i in hand.armIndices]
if not cspace.feasible(q0arm):
print "Warning, arm start configuration is infeasible"
print "TODO: Complete 2.a to bypass this error"
raw_input()
cspace.close()
return None
#TODO: get the ungrasp config using an IK solver
qungrasp = None
qungrasparm = None
print "TODO: Complete 2.b to find a feasible ungrasp config"
raw_input()
solver = hand.ikSolver(robot, vectorops.add(obj.getTransform()[1], shift),
(0, 0, 1))
#plan the transfer path between q0arm and qungrasparm
print "Planning transfer motion to ungrasp config..."
MotionPlan.setOptions(connectionThreshold=5.0, perturbationRadius=0.5)
planner = MotionPlan(cspace, 'sbl')
planner.setEndpoints(q0arm, qungrasparm)
#TODO: do the planning
print "TODO: Complete 2.c to find a feasible transfer path"
raw_input()
cspace.close()
#lift arm path to whole configuration space path
path = []
for qarm in planner.getPath():
path.append(q0[:])
for qi, i in zip(qarm, hand.armIndices):
path[-1][i] = qi
qpostungrasp = hand.open(qungrasp, 1.0)
return path + [qpostungrasp]
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 solve_placement(self):
"""Implemented for you: come up with a collision-free target placement"""
obmin,obmax = self.objbb
ozmin = obmin[2]
min_dims = min(obmax[0]-obmin[0],obmax[1]-obmin[1])
center = [0.5*(obmax[0]+obmin[0]),0.5*(obmax[1]-obmin[1])]
xmin,ymin,zmin = self.goal_bounds[0]
xmax,ymax,zmax = self.goal_bounds[1]
center_sampling_range = [(xmin+min_dims/2,xmax-min_dims/2),(ymin+min_dims/2,ymax-min_dims/2)]
Tobj_feasible = []
for iters in range(20):
crand = [random.uniform(b[0],b[1]) for b in center_sampling_range]
Robj = so3.rotation((0,0,1),random.uniform(0,math.pi*2))
tobj = vectorops.add(so3.apply(Robj,[-center[0],-center[1],0]),[crand[0],crand[1],zmin-ozmin+0.002])
self.object.setTransform(Robj,tobj)
feasible = True
for i in range(self.world.numTerrains()):
if self.object.geometry().collides(self.world.terrain(i).geometry()):
feasible=False
break
if not feasible:
bmin,bmax = self.object.geometry().getBBTight()
if bmin[0] < xmin:
tobj[0] += xmin-bmin[0]
if bmax[0] > xmax:
tobj[0] -= bmin[0]-xmax
if bmin[1] < ymin:
tobj[1] += ymin-bmin[1]
if bmax[1] > ymax:
tobj[1] -= bmin[1]-ymax
self.object.setTransform(Robj,tobj)
feasible = True
for i in range(self.world.numTerrains()):
if self.object.geometry().collides(self.world.terrain(i).geometry()):
feasible=False
break
if not feasible:
continue
for i in range(self.world.numRigidObjects()):
if i == self.object.index: continue
if self.object.geometry().collides(self.world.rigidObject(i).geometry()):
#raise it up a bit
bmin,bmax = self.world.rigidObject(i).geometry().getBB()
tobj[2] = bmax[2]-ozmin+0.002
self.object.setTransform(Robj,tobj)
Tobj_feasible.append((Robj,tobj))
print("Found",len(Tobj_feasible),"valid object placements")
return Tobj_feasible
def make_object_arrangement(world,container,objects,container_wall_thickness=0.01,max_iterations=100,remove_failures=False):
"""For a given container and a list of objects in the world, places the objects inside the container with randomized x-y locations
and z orientations so that they are initially collision free and on the bottom of the container.
Args:
world (WorldModel): the world containing the objects and obstacles
container: the container RigidObject / Terrain in world into which
objects should be spawned. Assumed axis-aligned.
objects (list of RigidObject): a list of RigidObjects in the world,
at arbitrary locations. They are placed in order.
container_wall_thickness (float, optional): a margin subtracted from
the container's outer dimensions into which the objects are spawned.
max_iterations (int, optional): the maximum number of iterations used
for sampling object initial poses
remove_failures (bool): if True, then instead of returning None on
failure, the objects that fail placement are removed from the world.
Returns:
(WorldModel): if successful, the positions of objects in world are
modified and world is returned. On failure, None is returned.
Note:
Since world is modified in-place, if you wish to make multiple worlds with
piles of the same objects, you should use world.copy() to store the
configuration of the objects. You may also wish to randomize the object
ordering using random.shuffle(objects) between instances.
"""
container_outer_bb = _get_bound(container)
container_inner_bb = (vectorops.add(container_outer_bb[0],[container_wall_thickness]*3),vectorops.sub(container_outer_bb[1],[container_wall_thickness]*3))
collision_margin = 0.0025
for object in objects:
#make sure the bottom of the object touches the bottom of the container
obb = _get_bound(object)
zmin = obb[0][2]
R,t = object.getTransform()
t[2] = container_inner_bb[2] - zmin + collision_margin
object.setTransform(R,t)
failures = xy_jiggle(world,rigid_objects,[container],container_inner_bb[0],container_inner_bb[1],max_iterations)
if len(failures) > 0:
if remove_failures:
removeIDs = [objects[i].index for i in failures]
for i in sorted(removeIDs)[::-1]:
world.remove(world.rigidObject(i))
else:
return None
return world
def make_primitive_object(world, typename, name=None):
"""Adds an object to the world using its geometry / mass properties
and places it in a default location (x,y)=(0,0) and resting on plane."""
if name is None:
name = typename
fn = "data/objects/" + typename + ".obj"
if not world.readFile(fn):
raise IOError("Unable to read primitive from file", fn)
T = obj.getTransform()
spacing = 0.006
T = (T[0],
vectorops.add(T[1], (-(bmin[0] + bmax[0]) * 0.5,
-(bmin[1] + bmax[1]) * 0.5, -bmin[2] + spacing)))
obj.setTransform(*T)
obj.appearance().setColor(0.2, 0.5, 0.7, 1.0)
obj.setName(name)
return obj
def retract(robot,gripper,amount,local=True):
"""Retracts the robot's gripper by a vector `amount`.
if local=True, amount is given in local coordinates. Otherwise, its given in
world coordinates.
"""
if not isinstance(gripper,(int,str)):
gripper = gripper.base_link
link = robot.link(gripper)
Tcur = link.getTransform()
if local:
amount = so3.apply(Tcur[0],amount)
obj = ik.objective(link,R=Tcur[0],t=vectorops.add(Tcur[1],amount))
res = ik.solve(obj)
if not res:
return None
return robot.getConfig()
def control_loop(self):
#Calculate the desired velocity for each robot by adding up all
#commands
rvels = [[0]*self.world.robot(r).numLinks() for r in range(self.world.numRobots())]
for (c,(r,v)) in self.current_velocities.iteritems():
rvels[r] = vectorops.add(rvels[r],v)
#print rvels
#send to the robot(s)
for r in range(self.world.numRobots()):
robotController = self.sim.controller(r)
qdes = robotController.getCommandedConfig()
qdes = vectorops.madd(qdes,rvels[r],self.dt)
#clamp to joint limits
(qmin,qmax) = self.world.robot(r).getJointLimits()
for i in xrange(len(qdes)):
qdes[i] = min(qmax[i],max(qdes[i],qmin[i]))
robotController.setPIDCommand(qdes,rvels[r])
return
def getDesiredCartesianPose(self,limb,device):
"""Returns a pair (R,t) of the desired EE pose if the limb should have
a cartesian pose message, or None if it should not.
- limb: either 'left' or 'right'
- device: an index of the haptic device
Implementation-wise, this reads from self.startTransforms and the deviceState
to determine the correct desired end effector transform. The delta
from devices[device]['deviceCurrentTransform'] to devices[device]['deviceInitialTransform']
is scaled, then offset by self.startTransforms[device]. (self.startTransforms is
the end effector transform when deviceInitialTransform is set)
"""
if limb=='left':
T = self.serviceThread.eeGetter_left.get()
else:
T = self.serviceThread.eeGetter_right.get()
if T is None:
T = se3.identity()
R,t=T
deviceState = self.haptic_client.deviceState
if deviceState == None: return T
dstate = deviceState[device]
if dstate.widgetInitialTransform[0] == None or self.startTransforms[device] == None: return T
Tcur = dstate.widgetCurrentTransform
T0 = dstate.widgetInitialTransform[0]
if T0 == None:
T0 = Tcur
#print "Button is down and mode is",dstate['mode']
#print dstate
assert T0 != None,"T0 is null"
assert Tcur != None,"Tcur is null"
relRot = so3.mul(Tcur[0],so3.inv(T0[0]))
relTrans = vectorops.sub(Tcur[1],T0[1])
#print "Rotation moment",so3.moment(relRot)
desRot = so3.mul(relRot,self.startTransforms[device][0])
desPos = vectorops.add(vectorops.mul(relTrans,hapticArmTranslationScaling),self.startTransforms[device][1])
#TEST: just use start rotation
#desRot = self.startTransforms[i][0]
return (desRot,desPos)
def next_state(self,x,u):
assert len(x) == 3
assert len(u) == 2
pos = [x[0],x[1]]
fwd = [math.cos(x[2]),math.sin(x[2])]
right = [-fwd[1],fwd[0]]
phi = u[1]
d = u[0]
if abs(phi)<1e-8:
newpos = vectorops.madd(pos,fwd,d)
return newpos + [x[2]]
else:
#rotate about a center of rotation, with radius 1/phi
cor = vectorops.madd(pos,right,1.0/phi)
sign = cmp(d*phi,0)
d = abs(d)
phi = abs(phi)
theta=0
thetaMax=d*phi
newpos = vectorops.add(so2.apply(sign*thetaMax,vectorops.sub(pos,cor)),cor)
return newpos + [so2.normalize(x[2]+sign*thetaMax)]
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 make_ycb_object(objectname, world, textured=True):
"""Adds an object to the world using its geometry / mass properties
and places it in a default location (x,y)=(0,0) and resting on plane."""
if objectname == 'random':
objectname = random.choice(ycb_objects)
if isinstance(objectname, int):
objectname = ycb_objects[objectname]
objmass = ycb_masses.get(objectname, DEFAULT_OBJECT_MASS)
if textured:
fn = os.path.join(YCB_DIR, objectname, 'textured.obj')
else:
fn = os.path.join(YCB_DIR, objectname, 'nontextured.ply')
obj = world.makeRigidObject(objectname)
if not obj.geometry().loadFile(fn):
raise IOError("Unable to read geometry from file", fn)
obj.setTransform(*se3.identity())
#set mass automatically
mass = obj.getMass()
surfaceFraction = 0.5
mass.estimate(obj.geometry(), objmass, surfaceFraction)
obj.setMass(mass)
bmin, bmax = obj.geometry().getBB()
T = obj.getTransform()
spacing = 0.006
T = (T[0],
vectorops.add(T[1], (-(bmin[0] + bmax[0]) * 0.5,
-(bmin[1] + bmax[1]) * 0.5, -bmin[2] + spacing)))
obj.setTransform(*T)
if textured:
obj.appearance().setColor(1, 1, 1, 1.0)
else:
obj.appearance().setColor(random.random(), random.random(),
random.random(), 1.0)
obj.setName(objectname)
return obj
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 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 get_contact_forces_and_jacobians(self):
"""
Returns a force contact vector 1x(6*n_contacts)
and a contact jacobian matrix 6*n_contactsxn.
Contact forces are considered to be applied at the link origin
"""
n_contacts = 0 # one contact per link
maxid = self.world.numIDs()
J_l = dict()
f_l = dict()
t_l = dict()
for l_id in self.u_to_l:
l_index = self.l_to_i[l_id]
link_in_contact = self.robot.link(l_index)
contacts_per_link = 0
for j in xrange(maxid): # TODO compute just one contact per link
contacts_l_id_j = len(self.sim.getContacts(l_id, j))
contacts_per_link += contacts_l_id_j
if contacts_l_id_j > 0:
if not f_l.has_key(l_id):
f_l[l_id] = self.sim.contactForce(l_id, j)
t_l[l_id] = self.sim.contactTorque(l_id, j)
else:
f_l[l_id] = vectorops.add(f_l[l_id], self.sim.contactForce(l_id, j))
t_l[l_id] = vectorops.add(t_l[l_id], self.sim.contactTorque(l_id, j))
### debugging ###
# print "link", link_in_contact.getName(), """
# in contact with obj""", self.world.getName(j), """
# (""", len(self.sim.getContacts(l_id, j)), """
# contacts)\n f=""",self.sim.contactForce(l_id, j), """
# t=""", self.sim.contactTorque(l_id, j)
if self.virtual_contacts.has_key(l_id):
b = self.sim.body(link_in_contact)
f_v = se3.apply_rotation(b.getTransform(),self.virtual_wrenches[l_id][0:3])
t_v = se3.apply_rotation(b.getTransform(),self.virtual_wrenches[l_id][3:6])
if not f_l.has_key(l_id):
f_l[l_id] = f_v
t_l[l_id] = t_v
else:
f_l[l_id] = f_v
t_l[l_id] = t_v
### debugging ###
"""
if contacts_per_link == 0:
print "link", link_in_contact.getName(), "not in contact"
"""
if contacts_per_link > 0 or self.virtual_contacts.has_key(l_id):
n_contacts += 1
J_l[l_id] = np.array(link_in_contact.getJacobian(
(0, 0, 0)))
self.robot.setConfig(self.sim.getActualConfig(self.robotindex))
#print "J_l[l_id]:\n",J_l[l_id]
#print "J_l shape", J_l[l_id].shape
f_c = np.array(6 * n_contacts * [0.0])
J_c = np.zeros((6 * n_contacts, self.u_dofs))
for l_in_contact in xrange(len(J_l.keys())):
f_c[l_in_contact * 6:l_in_contact * 6 + 3
] = t_l.values()[l_in_contact] # Jacobian has angular velocity first, then linear
f_c[l_in_contact * 6 + 3:l_in_contact * 6 + 6
] = f_l.values()[l_in_contact] # Jacobian has angular velocity first, then linear
J_c[l_in_contact * 6:l_in_contact * 6 + 6,
:] = np.array(
J_l.values()[l_in_contact])[:, [self.q_to_t[u_id] for u_id in self.u_to_n]]
#print f_c, J_c
return (f_c, J_c)
def get_contact_forces_and_jacobians(self):
"""
Returns a force contact vector 1x(6*n_contacts)
and a contact jacobian matrix 6*n_contactsxn.
Contact forces are considered to be applied at the link origin
"""
n_contacts = 0 # one contact per link
maxid = self.world.numIDs()
J_l = dict()
f_l = dict()
t_l = dict()
for l_id in self.u_to_l:
l_index = self.l_to_i[l_id]
link_in_contact = self.robot.link(l_index)
contacts_per_link = 0
for j in xrange(maxid): # TODO compute just one contact per link
contacts_l_id_j = len(self.sim.getContacts(l_id, j))
contacts_per_link += contacts_l_id_j
if contacts_l_id_j > 0:
if not f_l.has_key(l_id):
f_l[l_id] = self.sim.contactForce(l_id, j)
t_l[l_id] = self.sim.contactTorque(l_id, j)
else:
f_l[l_id] = vectorops.add(f_l[l_id], self.sim.contactForce(l_id, j))
t_l[l_id] = vectorops.add(t_l[l_id], self.sim.contactTorque(l_id, j))
### debugging ###
# print "link", link_in_contact.getName(), """
# in contact with obj""", self.world.getName(j), """
# (""", len(self.sim.getContacts(l_id, j)), """
# contacts)\n f=""",self.sim.contactForce(l_id, j), """
# t=""", self.sim.contactTorque(l_id, j)
if self.virtual_contacts.has_key(l_id):
b = self.sim.body(link_in_contact)
f_v = se3.apply_rotation(b.getTransform(),self.virtual_wrenches[l_id][0:3])
t_v = se3.apply_rotation(b.getTransform(),self.virtual_wrenches[l_id][3:6])
if not f_l.has_key(l_id):
f_l[l_id] = f_v
t_l[l_id] = t_v
else:
f_l[l_id] += f_v
t_l[l_id] += t_v
### debugging ###
"""
if contacts_per_link == 0:
print "link", link_in_contact.getName(), "not in contact"
"""
if contacts_per_link > 0 or self.virtual_contacts.has_key(l_id):
n_contacts += 1
J_l[l_id] = np.array(link_in_contact.getJacobian(
(0, 0, 0)))
self.robot.setConfig(self.sim.getActualConfig(self.robotindex))
#print "J_l[l_id]:\n",J_l[l_id]
#print "J_l shape", J_l[l_id].shape
f_c = np.array(6 * n_contacts * [0.0])
J_c = np.zeros((6 * n_contacts, self.u_dofs))
for l_in_contact in xrange(len(J_l.keys())):
f_c[l_in_contact * 6:l_in_contact * 6 + 3
] = t_l.values()[l_in_contact] # Jacobian has angular velocity first, then linear
f_c[l_in_contact * 6 + 3:l_in_contact * 6 + 6
] = f_l.values()[l_in_contact] # Jacobian has angular velocity first, then linear
J_c[l_in_contact * 6:l_in_contact * 6 + 6,
:] = np.array(
J_l.values()[l_in_contact])[:, [self.q_to_t[u_id] for u_id in self.u_to_n]]
#print f_c, J_c
return (f_c, J_c)
def to_povray(vis, world, properties={}):
"""Convert a frame in klampt visualization to a povray script (or render
to an image)
Args:
vis: sub-class of GLProgram
world: sub-class of WorldModel
properties: some additional information that povray can take but does
not map to anything in klampt.
Note::
Do not modify properties, use the convenience functions that I provide
below (after this function). These take the form ``render_to_x`` and
``add_x``.
"""
#patch on vapory
patch_vapory()
#camera
mat = vis.view.camera.matrix()
pos = mat[1]
right = mat[0][0:3]
up = mat[0][3:6]
dir = op.mul(mat[0][6:9], -1)
tgt = op.add(mat[1], dir)
#scale
fovy = vis.view.fov * vis.view.h / vis.view.w
fovx = math.atan(vis.view.w * math.tan(fovy * math.pi / 360.) /
vis.view.h) * 360. / math.pi
right = op.mul(right, -float(vis.view.w) / vis.view.h)
#camera
camera_params = [
'orthographic' if vis.view.orthogonal else 'perspective', 'location',
[pos[0], pos[1], pos[2]], 'look_at', [tgt[0], tgt[1], tgt[2]], 'right',
[right[0], right[1], right[2]], 'up', [up[0], up[1], up[2]], 'angle',
fovx, 'sky',
get_property(properties, [], "sky", [0., 0., 1.])
]
camera = vp.Camera(*camera_params)
#tempfile
tempfile = get_property(properties, [], "tempfile", None)
tempfile_path = os.path.dirname(tempfile) if tempfile is not None else '.'
if not os.path.exists(tempfile_path):
os.mkdir(tempfile_path)
#objects
objects = []
objs = [o for o in properties["visualObjects"]
] if "visualObjects" in properties else []
objs += [world.terrain(i) for i in range(world.numTerrains())]
objs += [world.rigidObject(i) for i in range(world.numRigidObjects())]
for r in range(world.numRobots()):
objs += [
world.robot(r).link(i) for i in range(world.robot(r).numLinks())
]
for obj in objs:
transient = get_property(properties, [obj], "transient", default=True)
if transient:
objects += geometry_to_povray(obj.appearance(),
obj.geometry(),
obj,
None,
properties=properties)
else:
path = tempfile_path + '/' + obj.getName() + '.pov'
if not os.path.exists(path):
R, t = obj.geometry().getCurrentTransform()
obj.geometry().setCurrentTransform([1, 0, 0, 0, 1, 0, 0, 0, 1],
[0, 0, 0])
geom = geometry_to_povray(obj.appearance(),
obj.geometry(),
obj,
None,
properties=properties)
if len(geom) > 1:
file_content = vp.Union(*geom)
elif len(geom) > 0:
file_content = vp.Object(*geom)
else:
file_content = None
if file_content is not None:
f = open(path, 'w')
f.write(str(file_content))
f.close()
obj.geometry().setCurrentTransform(R, t)
else:
path = None
#include
if path is not None:
R, t = obj.geometry().getCurrentTransform()
objects.append(
vp.Object('#include "%s"' % path, "matrix", R + t))
#light
if "lights" in properties:
objects += properties["lights"]
#scene
gsettings = []
scene = vp.Scene(camera=camera,
objects=objects,
included=get_property(properties, [], "included", []),
global_settings=get_property(properties, [],
"global_settings", []))
try:
#this works with later version of vapory
return \
render_povstring(str(scene), \
outfile=get_property(properties,[],"outfile",None), \
width=vis.view.w,height=vis.view.h, \
quality=get_property(properties,[],"quality",None), \
antialiasing=get_property(properties,[],"antialiasing",0.3), \
remove_temp=get_property(properties,[],"remove_temp",False), \
show_window=get_property(properties,[],"show_window",False), \
tempfile=tempfile, \
includedirs=get_property(properties,[],"includedirs",None), \
output_alpha=get_property(properties,[],"output_alpha",True))
except:
#this works with earlier version of vapory
return \
render_povstring(str(scene), \
outfile=get_property(properties,[],"outfile",None), \
width=vis.view.w,height=vis.view.h, \
quality=get_property(properties,[],"quality",None), \
antialiasing=get_property(properties,[],"antialiasing",0.3), \
remove_temp=get_property(properties,[],"remove_temp",False))
#visualize(world, robot, start=start,goal=goal)
qmin, qmax = robot.getJointLimits()
qmin[2] = -math.pi / 2
qmax[2] = 0
qmin[3] = 0
qmax[3] = math.pi
robot.setJointLimits(qmin, qmax)
config = robot.getConfig()
config[:] = q0[:len(config)].tolist()
robot.setConfig(config)
end_link = robot.link(6) # for redundancy
end_link_pos = end_link.getWorldPosition([0, 0, 0])
object_pos = vo.add(end_link_pos, [0, 0.0, 0.18])
print('object at ', object_pos)
box = create.primitives.box(0.1,
0.1,
0.1,
object_pos,
world=world,
name='box',
mass=3)
print(type(box))
print(robot.getConfig())
path = np.zeros((plan.shape[0], len(config)))
path[:] = plan[:, :len(config)]
path = path.tolist()
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 drawRobotGL(self, q):
glColor3f(0, 0, 1)
newc = vectorops.add(self.robot.center, q)
c = Circle(newc[0], newc[1], self.robot.radius)
c.drawGL()
def make_object_pile(world,container,objects,container_wall_thickness=0.01,randomize_orientation=True,
visualize=False,verbose=0):
"""For a given container and a list of objects in the world, drops the objects inside the container and simulates until stable.
Args:
world (WorldModel): the world containing the objects and obstacles
container: the container RigidObject / Terrain in world into which
objects should be spawned. Assumed axis-aligned.
objects (list of RigidObject): a list of RigidObjects in the world,
at arbitrary locations. They are placed in order.
container_wall_thickness (float, optional): a margin subtracted from
the container's outer dimensions into which the objects are spawned.
randomize_orientation (bool or str, optional): if True, the orientation
of the objects are completely randomized. If 'z', only the z
orientation is randomized. If False or None, the orientation is
unchanged
visualize (bool, optional): if True, pops up a visualization window to
show the progress of the pile
verbose (int, optional): if > 0, prints progress of the pile.
Side effect: the positions of objects in world are modified
Returns:
(tuple): (world,sim), containing
- world (WorldModel): the original world
- sim (Simulator): the Simulator instance at the state used to obtain
the stable placement of the objects.
Note:
Since world is modified in-place, if you wish to make multiple worlds with
piles of the same objects, you should use world.copy() to store the
configuration of the objects. You may also wish to randomize the object
ordering using random.shuffle(objects) between instances.
"""
container_outer_bb = _get_bound(container)
container_inner_bb = (vectorops.add(container_outer_bb[0],[container_wall_thickness]*3),vectorops.sub(container_outer_bb[1],[container_wall_thickness]*3))
spawn_area = (container_inner_bb[0][:],container_inner_bb[1][:])
collision_margin = 0.0025
if visualize:
from klampt import vis
from klampt.model import config
import time
oldwindow = vis.getWindow()
newwindow = vis.createWindow("make_object_pile dynamic visualization")
vis.setWindow(newwindow)
vis.show()
visworld = world.copy()
vis.add("world",visworld)
sim = Simulator(world)
sim.setSetting("maxContacts","20")
sim.setSetting("adaptiveTimeStepping","0")
Tfar = (so3.identity(),[0,0,-100000])
for object in objects:
R,t = object.getTransform()
object.setTransform(R,Tfar[1])
sim.body(object).setTransform(*Tfar)
sim.body(object).enable(False)
if verbose:
print("Spawn area",spawn_area)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
for index in range(len(objects)):
#always spawn above the current height of the pile
if index > 0:
objects_bound = _get_bound(objects[:index])
if verbose:
print("Existing objects bound:",objects_bound)
zshift = max(0.0,objects_bound[1][2] - spawn_area[0][2])
spawn_area[0][2] += zshift
spawn_area[1][2] += zshift
object = objects[index]
obb = _get_bound(object)
zmin = obb[0][2]
R0,t0 = object.getTransform()
feasible = False
for sample in range(1000):
R,t = R0[:],t0[:]
if randomize_orientation == True:
R = so3.sample()
t[2] = spawn_area[1][2] - zmin + t0[2] + collision_margin
object.setTransform(R,t)
xy_randomize(object,spawn_area[0],spawn_area[1])
if verbose:
print("Sampled position of",object.getName(),object.getTransform()[1])
if not randomize_orientation:
_,t = object.getTransform()
object.setTransform(R,t)
#object spawned, now settle
sobject = sim.body(object)
sobject.enable(True)
sobject.setTransform(*object.getTransform())
res = sim.checkObjectOverlap()
if len(res[0]) == 0:
feasible = True
#get it low as possible without overlapping
R,t = object.getTransform()
for lower in range(100):
sobject.setTransform(R,vectorops.add(t,[0,0,-(lower+1)*0.01]))
res = sim.checkObjectOverlap()
if len(res[0]) != 0:
if verbose:
print("Terminated lowering at",lower,"cm lower")
sobject.setTransform(R,vectorops.add(t,[0,0,-lower*0.01]))
res = sim.checkObjectOverlap()
break
sim.updateWorld()
break
if not feasible:
if verbose:
print("Failed to place object",object.getName())
return None
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.1)
if verbose:
print("Beginning to simulate")
#start letting everything fall
for firstfall in range(10):
sim.simulate(0.01)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(0.01)
maxT = 5.0
dt = 0.01
t = 0.0
wdamping = -0.01
vdamping = -0.1
while t < maxT:
settled = True
for object in objects:
sobject = sim.body(object)
w,v = sobject.getVelocity()
sobject.applyWrench(vectorops.mul(v,vdamping),vectorops.mul(w,wdamping))
if vectorops.norm(w) + vectorops.norm(v) > 1e-4:
#settled
settled=False
break
if settled:
break
if visualize:
t0 = time.time()
sim.simulate(dt)
if visualize:
vis.lock()
config.setConfig(visworld,config.getConfig(world))
vis.unlock()
time.sleep(max(0.0,dt-(time.time()-t0)))
t += dt
if visualize:
vis.show(False)
vis.setWindow(oldwindow)
return (world,sim)
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
