def drawContactForces(self):
        contacted=False;
        for i in range(self.simworld.numIDs()):
            for j in range(i+1, self.simworld.numIDs()):
                if(self.sim.hadContact(i,j)):
                    if(not contacted):
                        # print "Touching bodies:\n"
                        contacted=True
                    f = self.sim.meanContactForce(i,j)
                    contacts = self.sim.getContacts(i,j)

                    # print self.simworld.getName(i),self.simworld.getName(j), len(contacts)
                    x=[0,0,0]
                    n=[0,0,0]
                    k=0
                    scale = 5

                    if len(contacts) != 0:
                        for numContact in range(len(contacts)):
                            x = vectorops.add(x,contacts[numContact][0:3])
                            n = vectorops.add(n,contacts[numContact][3:6])
                            k += contacts[numContact][6]
                        x = vectorops.div(x,len(contacts))
                        n = vectorops.div(n,len(contacts))
                        k = k/(len(contacts)*scale)
                        draw_vector(x,n,k)
def angle(a, b):
    anorm = vectorops.norm(a)
    bnorm = vectorops.norm(b)
    if anorm < 1e-6 or bnorm < 1e-6: return 0
    dp = vectorops.dot(vectorops.div(a, anorm), vectorops.div(b, bnorm))
    dp = max(min(dp, 1), -1)
    return math.acos(dp)
Exemple #3
0
    def saveStep(self, extra=[]):
        sim = self.sim
        world = sim.world
        sim.updateWorld()
        values = []
        values.append(sim.getTime())
        for i in xrange(world.numRobots()):
            robot = world.robot(i)
            values += robot.getCom()
            values += robot.getConfig()
            values += robot.getVelocity()
            values += sim.getActualTorques(i)
            j = 0
            while True:
                s = self.sim.controller(i).sensor(j)
                if s is None or len(s.name()) == 0:
                    break
                meas = s.getMeasurements()
                values += meas
                j += 1
        for i in xrange(world.numRigidObjects()):
            obj = world.rigidObject(i)
            T = obj.getTransform()
            values += se3.apply(T, obj.getMass().getCom())
            values += T[1]
            values += so3.moment(T[0])
            values += sim.body(obj).getVelocity()[1]
            values += sim.body(obj).getVelocity()[0]

        if self.f_contact:
            for i, id in enumerate(self.colliding):
                for j in range(i + 1, len(self.colliding)):
                    id2 = self.colliding[j]
                    if sim.hadContact(id, id2):
                        clist = sim.getContacts(id, id2)
                        f = sim.contactForce(id, id2)
                        m = sim.contactTorque(id, id2)
                        pavg = [0.0] * 3
                        navg = [0.0] * 3
                        for c in clist:
                            pavg = vectorops.add(pavg, c[0:3])
                            navg = vectorops.add(navg, c[3:6])
                        if len(clist) > 0:
                            pavg = vectorops.div(pavg, len(clist))
                            navg = vectorops.div(navg, len(clist))
                        body1 = world.getName(id)
                        body2 = world.getName(id2)
                        cvalues = [sim.getTime(), body1, body2, len(clist)]
                        cvalues += pavg
                        cvalues += navg
                        cvalues += f
                        cvalues += m
                        self.f_contact.write(','.join(str(v) for v in cvalues))
                        self.f_contact.write('\n')
        if extra:
            values += extra
        if not (self.f is None):
            self.f.write(','.join([str(v) for v in values]))
            self.f.write('\n')
Exemple #4
0
 def saveStep(self,extra=[]):
     sim = self.sim
     world = sim.world
     sim.updateWorld()
     values = []
     values.append(sim.getTime())
     for i in xrange(world.numRobots()):
         robot = world.robot(i)
         values += robot.getCom()
         values += robot.getConfig()
         values += sim.getActualTorques(i)
         """
         j = 0
         while True:
             s = self.sim.controller(i).getSensor(j)
             if len(s.name())==0:
                 break
             meas = s.measurements()
             values += meas
             j += 1
         """
     for i in xrange(world.numRigidObjects()):
         obj = world.rigidObject(i)
         T = obj.getTransform()
         values += se3.apply(T,obj.getMass().getCom())
         values += T[1]
         values += so3.moment(T)
         values += sim.body(obj).getVelocity()[1]
         values += sim.body(obj).getVelocity()[0]
     
     if self.f_contact:
         for i,id in enumerate(self.colliding):
             for j in range(i+1,len(self.colliding)):
                 id2 = self.colliding[j]
                 if sim.hadContact(id,id2):
                     clist = sim.getContacts(id,id2);
                     f = sim.contactForce(id,id2)
                     m = sim.contactTorque(id,id2)
                     pavg = [0.0]*3
                     navg = [0.0]*3
                     for c in clist:
                         pavg = vectorops.add(pavg,c[0:3])
                         navg = vectorops.add(navg,c[3:6])
                     if len(clist) > 0:
                         pavg = vectorops.div(pavg,len(clist))
                         navg = vectorops.div(navg,len(clist))
                     body1 = world.getName(id)
                     body2 = world.getName(id2)
                     values = [sim.getTime(),body1,body2,len(clist)]
                     values += pavg
                     values += navg
                     values += f
                     values += m
                     self.f_contact.write(','.join(str(v) for v in values))
                     self.f_contact.write('\n')
     if extra:
         values += extra
     self.f.write(','.join([str(v) for v in values]))
     self.f.write('\n')
Exemple #5
0
    def sendPath(path,maxSmoothIters =0, INCREMENTAL=False, limb = None, readConstants=False, internalSpeed=SPEED):


        # interpolate path linearly between two endpoints
        if path == None:
            print "sending empty path"
            return False

        for smoothIter in range(maxSmoothIters):
            # path = path
            smoothePath = [0]*(len(path)*2-1)
            for i in range(len(path)-1):
                smoothePath[i*2] = path[i]
                smoothePath[i*2 +1] = vectorops.div(vectorops.add(path[i],path[i+1]), 2)
            smoothePath[-1] = path[-1]
            path = smoothePath



        while i <endIndex-1:
            # print i, endIndex
            q = path[i]
            qNext = path[i+1]
            dt = vectorops.distance(q,qNext)

            # smooth trajectory by interpolating between two consecutive configurations
            # if the distance between the two is big

            # Note: might want to make dt bigger for arm movements (only 7 configurations vs 52)
            if dt>0.1:
                qInterp = vectorops.div(vectorops.add(q, qNext), 2)
                path.insert(i+1, qInterp)
                endIndex +=1
                continue

            else:
                i += 1
                waitForMove()
                if counter%internalSpeed == 0 or INCREMENTAL:
                    if limb == 'left':
                        #CONTROLLER.appendMilestoneLeft(q)
                        pass
                    elif limb == 'right':
                        #CONTROLLER.appendMilestoneRight(q)
                        pass
                    else:
                        #print 'milestone #', i, q
                        #CONTROLLER.appendMilestone(q)
                        pass
                counter +=1
        if limb == 'left':
            #CONTROLLER.appendMilestoneLeft(path[-1])
            pass
        elif limb == 'right':
            #CONTROLLER.appendMilestoneRight(path[-1])
            pass
        else:
            pass
Exemple #6
0
def point_fit_xform_3d(apts,bpts):
    """Finds a 3D rigid transform that maps the list of points apts to the
    list of points bpts.  Return value is a klampt.se3 element that
    minimizes the sum of squared errors ||T*ai-bi||^2.
    """
    assert len(apts)==len(bpts)
    ca = vectorops.div(reduce(apts,vectorops.add),len(apts))
    cb = vectorops.div(reduce(bpts,vectorops.add),len(bpts))
    arel = [vectorops.sub(a,ca) for a in apts]
    brel = [vectorops.sub(b,cb) for b in bpts]
    R = point_fit_rotation_3d(arel,brel)
    #R minimizes sum_{i=1,...,n} ||R(ai-ca) - (bi-cb)||^2
    t = so3.sub(cb,so3.apply(R,ca))
    return (R,t)
Exemple #7
0
def point_fit_xform_3d(apts,bpts):
    """Finds a 3D rigid transform that maps the list of points apts to the
    list of points bpts.  Return value is a klampt.se3 element that
    minimizes the sum of squared errors ||T*ai-bi||^2.
    """
    assert len(apts)==len(bpts)
    ca = vectorops.div(reduce(apts,vectorops.add),len(apts))
    cb = vectorops.div(reduce(bpts,vectorops.add),len(bpts))
    arel = [vectorops.sub(a,ca) for a in apts]
    brel = [vectorops.sub(b,cb) for b in bpts]
    R = point_fit_rotation_3d(arel,brel)
    #R minimizes sum_{i=1,...,n} ||R(ai-ca) - (bi-cb)||^2
    t = so3.sub(cb,so3.apply(R,ca))
    return (R,t)
	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
Exemple #9
0
 def output_and_advance(self, **inputs):
     try:
         q = inputs['q']
         dq = inputs['dq']
         u = vectorops.div(vectorops.sub(inputs['qcmd'], q), inputs['dt'])
     except KeyError:
         print "Warning, cannot debug motion model, dq or dqcmd not in input"
         return None
     if self.dqpredlast != None:
         if self.activeDofs != None:
             dq = dq[:]
             for i in [
                     i for i in range(len(q)) if i not in self.activeDofs
             ]:
                 dq[i] = self.dqpredlast[i]
         #compare motion model to dq
         print "Motion model error:", np.linalg.norm(self.dqpredlast -
                                                     np.array(dq))
         (v, i) = max(
             zip(
                 np.abs(self.dqpredlast - np.array(dq)).tolist(),
                 range(len(dq))))
         print "  Max error:", v, "at", i,
         if self.robot != None: print self.robot.getLink(i).getName()
         else: print
         print "  Command:", self.ulast[i], "Predicted:", self.dqpredlast[
             i], "Actual:", dq[i]
         print "  pred:", self.Alast[i, i], "*u +", self.blast[i]
Exemple #10
0
    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
Exemple #11
0
def spawn_objects_from_ground_truth(world):
    """For all ground_truth_items, spawns RigidObjects in the world
    according to their sizes / mass properties"""
    global ground_truth_items
    print "Initializing world objects"
    for item in ground_truth_items:
        obj = world.makeRigidObject(item.info.name)
        bmin,bmax = item.info.bmin,item.info.bmax
        center = vectorops.div(vectorops.add(bmin,bmax),2.0)
        m = obj.getMass()
        m.setMass(item.info.mass)
        m.setCom([0,0,0])
        m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],item.info.mass/12.0))
        obj.setMass(m)
        c = obj.getContactParameters()
        c.kFriction = 0.6
        c.kRestitution = 0.1;
        c.kStiffness = 100000
        c.kDamping = 100000
        obj.setContactParameters(c)
        cube = obj.geometry()
        if not cube.loadFile(os.path.join(model_dir,"cube.tri")):
            print "Error loading cube file",os.path.join(model_dir,"cube.tri")
            exit(1)
        scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]]
        translate = vectorops.sub(bmin,center)
        cube.transform(scale,translate)
        mesh = cube.getTriangleMesh()
        obj.setTransform(item.xform[0],item.xform[1])
    return
Exemple #12
0
 def putSensedObjectInWorld(self,item):
     #put this obejct in the world
     obj = self.world.makeRigidObject(item.info.name)
     bmin,bmax = item.info.bmin,item.info.bmax
     center = vectorops.div(vectorops.add(bmin,bmax),2.0)
     m = obj.getMass()
     m.setMass(item.info.mass)
     m.setCom([0,0,0])
     m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],item.info.mass/12.0))
     obj.setMass(m)
     c = obj.getContactParameters()
     c.kFriction = 0.6
     c.kRestitution = 0.1;
     c.kStiffness = 100000
     c.kDamping = 100000
     obj.setContactParameters(c)
     cube = obj.geometry()
     if not cube.loadFile(os.path.join(model_dir,"cube.tri")):
         print "Error loading cube file",os.path.join(model_dir,"cube.tri")
         exit(1)
     scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]]
     translate = vectorops.sub(bmin,center)
     cube.transform(scale,translate)
     mesh = cube.getTriangleMesh()
     obj.setTransform(item.xform[0],item.xform[1])
    def update_world_with_knowledge(self):
        '''Create objects in the planning for those added to the
        knowledge base.'''

        # look at all the bins
        for (name, contents) in self.knowledge.bin_contents.items():
            # only care above perceived for full bins
            if contents:

                # look at all the items in the bin
                for item in contents:
                    # check which ones are missing planning objects
                    if item.klampt_index is None:
                        # create an object for it
                        # code borrowed from spawn_objects_from_ground_truth
                        obj = self.world.makeRigidObject(item.info.name)
                        bmin,bmax = item.info.bmin,item.info.bmax
                        center = vectorops.div(vectorops.add(bmin,bmax),2.0)
                        cube = obj.geometry()
                        if not cube.loadFile(os.path.join(model_dir,"cube.tri")):
                            print "Error loading cube file",os.path.join(model_dir,"cube.tri")
                            exit(1)
                        scale = [bmax[0]-bmin[0],0,0,0,bmax[1]-bmin[1],0,0,0,bmax[2]-bmin[2]]
                        translate = vectorops.sub(bmin,center)
                        cube.transform(scale,translate)
                        obj.setTransform(item.xform[0],item.xform[1])

                        # note this item's corresponding object
                        item.klampt_index = obj.getID()

                        # update the collider
                        self.collider = WorldCollider(self.world)

                        print 'spawned planning model', obj.getName()
Exemple #14
0
def spawn_item_in_world(item,world):
    """Given an ItemInBin instance, spawns a RigidObject in the Klamp't
    world with its same geometry / size / mass properties.
    Returns the new object."""
    obj = world.makeRigidObject(item.info.name)
    bmin,bmax = item.info.bmin,item.info.bmax
    center = vectorops.div(vectorops.add(bmin,bmax),2.0)
    m = obj.getMass()
    m.setMass(item.info.mass)
    m.setCom([0,0,0])
    m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],item.info.mass/12.0))
    obj.setMass(m)
    c = obj.getContactParameters()
    #TODO: make these parameters dynamic
    c.kFriction = 0.6
    c.kRestitution = 0.1;
    c.kStiffness = 100000
    c.kDamping = 100000
    obj.setContactParameters(c)
    simgeometry = obj.geometry()
    #either copy directly (this line)...
    simgeometry.set(item.info.geometry)
    #or reload from file (this line)
    #load_item_geometry(item.info,simgeometry)
    obj.setTransform(item.xform[0],item.xform[1])
    return obj
Exemple #15
0
 def output_and_advance(self,**inputs):
     try:
         dt = inputs["dt"]
         q = inputs["q"]
     except KeyError:
         raise ValueError("Input needs to have configuration 'q' and timestep 'dt'")
     if len(q)==0: return None
     if self.qlast==None:
         dq = [0]*len(q)
     else:
         if self.robot==None:
             dq = vectorops.div(self.robot.sub(q,self.qlast),dt)
         else:
             assert(len(self.qlast)==len(q))
             dq = vectorops.div(self.robot.interpolate_deriv(self.qlast,q),dt)
     self.qlast = q
     return {'dq':dq}
Exemple #16
0
def interpolate_rotation(R1, R2, u):
    """Interpolate linearly between the two rotations R1 and R2.
    TODO: the current implementation doesn't work properly.  Why? """
    m1 = so3.moment(R1)
    m2 = so3.moment(R2)
    mu = interpolate_linear(m1, m2, u)
    angle = vectorops.norm(mu)
    axis = vectorops.div(mu, angle)
    return so3.rotation(axis, angle)
Exemple #17
0
def interpolate_rotation(R1,R2,u):
    """Interpolate linearly between the two rotations R1 and R2.
    TODO: the current implementation doesn't work properly.  Why? """
    m1 = so3.moment(R1)
    m2 = so3.moment(R2)
    mu = interpolate_linear(m1,m2,u)
    angle = vectorops.norm(mu)
    axis = vectorops.div(mu,angle)
    return so3.rotation(axis,angle)
Exemple #18
0
 def output_and_advance(self, **inputs):
     try:
         dt = inputs["dt"]
         q = inputs["q"]
     except KeyError:
         raise ValueError(
             "Input needs to have configuration 'q' and timestep 'dt'")
     if len(q) == 0: return None
     if self.qlast == None:
         dq = [0] * len(q)
     else:
         if self.robot == None:
             dq = vectorops.div(self.robot.sub(q, self.qlast), dt)
         else:
             assert (len(self.qlast) == len(q))
             dq = vectorops.div(self.robot.interpolate_deriv(self.qlast, q),
                                dt)
     self.qlast = q
     return {'dq': dq}
Exemple #19
0
    def getSensedVelocity(self, q, dq, dt):
        """Gets task velocity from sensed configuration q.
		Default implementation uses finite differencing.  Other
		implementations may use jacobian.
		"""
        #uncomment this to get a jacobian based technique
        #return np.dot(self.getJacobian(q),dq)
        if self.qLast == None:
            return None
        else:
            xlast = self.getSensedValue(self.qLast)
            xcur = self.getSensedValue(q)
            return vectorops.div(self.taskDifference(xcur, xlast), dt)
	def getSensedVelocity(self, q, dq, dt):
		"""Gets task velocity from sensed configuration q.
		Default implementation uses finite differencing.  Other
		implementations may use jacobian.
		"""
		#uncomment this to get a jacobian based technique
		#return np.dot(self.getJacobian(q),dq)
		if self.qLast==None:
			return None
		else:
			xlast = self.getSensedValue(self.qLast)
			xcur = self.getSensedValue(q)
			return vectorops.div(self.taskDifference(xcur,xlast),dt)
	def setDesiredVelocitiesFromDifference(self,qdes0,qdes1,dt,tasks=None):
		"""Sets all the tasks' desired velocities from a given pair
		of configurations separated by dt (e.g., to follow a reference
		trajectory).
		
		If the 'tasks' variable is provided, it should be a list of
		tasks for which the desired values should be set.
		"""
		if tasks == None:
			tasks = self.taskList
		for t in tasks:
			xdes0 = t.getSensedValue(qdes0)
			xdes1 = t.getSensedValue(qdes1)
			dx = vectorops.div(t.taskDifference(xdes1,xdes0),dt)
			t.setDesiredVelocity(dx)
Exemple #22
0
    def setDesiredVelocitiesFromDifference(self, qdes0, qdes1, dt, tasks=None):
        """Sets all the tasks' desired velocities from a given pair
		of configurations separated by dt (e.g., to follow a reference
		trajectory).
		
		If the 'tasks' variable is provided, it should be a list of
		tasks for which the desired values should be set.
		"""
        if tasks == None:
            tasks = self.taskList
        for t in tasks:
            xdes0 = t.getSensedValue(qdes0)
            xdes1 = t.getSensedValue(qdes1)
            dx = vectorops.div(t.taskDifference(xdes1, xdes0), dt)
            t.setDesiredVelocity(dx)
Exemple #23
0
    def advance(self, **inputs):
        try:
            qcmd = inputs['qcmd']
            q = inputs['q']
            dt = inputs['dt']
        except KeyError:
            return
        if len(q) == 0: return
        print len(qcmd), len(q)
        u = vectorops.sub(qcmd, q)
        u = vectorops.div(u, dt)
        print "u estimate:", u
        print "u estimate rarm:", u[26:33]

        try:
            q = inputs[self.xnames[0]]
            dq = inputs[self.xnames[1]]
        except KeyError as e:
            print "Warning, inputs do not contain x key", e
            return
        """
        try:
            u = sum((inputs[uname] for uname in self.unames),[])
        except KeyError as e:
            print "Warning, inputs do not contain u key",e
            #u = [0]*(len(x)/2)
            try:
                u = vectorops.sub(inputs['qcmd'],inputs['q'])
                u = vectorops.div(u,inputs['dt'])
            except KeyError as e:
                print "Warning, inputs do not contain qcmd key either",e
                u = [0]*(len(x)/2)
            """
        #self.xlast = x
        #return
        #do system ID
        if self.qlast != None:
            self.estimator.add(self.qlast, self.dqlast, u, q, dq)
            pass
        #update last state
        self.qlast = q
        self.dqlast = dq
        return
Exemple #24
0
    def advance(self,**inputs):
        try:
            qcmd = inputs['qcmd']
            q = inputs['q']
            dt = inputs['dt']
        except KeyError:
            return
        if len(q)==0: return
        print len(qcmd),len(q)
        u = vectorops.sub(qcmd,q)
        u = vectorops.div(u,dt)
        print "u estimate:",u
        print "u estimate rarm:",u[26:33]

        try:
            q = inputs[self.xnames[0]]
            dq = inputs[self.xnames[1]]
        except KeyError as e:
            print "Warning, inputs do not contain x key",e
            return
        """
        try:
            u = sum((inputs[uname] for uname in self.unames),[])
        except KeyError as e:
            print "Warning, inputs do not contain u key",e
            #u = [0]*(len(x)/2)
            try:
                u = vectorops.sub(inputs['qcmd'],inputs['q'])
                u = vectorops.div(u,inputs['dt'])
            except KeyError as e:
                print "Warning, inputs do not contain qcmd key either",e
                u = [0]*(len(x)/2)
            """
            #self.xlast = x
            #return
        #do system ID
        if self.qlast != None:
            self.estimator.add(self.qlast,self.dqlast,u,q,dq)
            pass
        #update last state
        self.qlast = q
        self.dqlast = dq
        return
Exemple #25
0
 def output_and_advance(self,**inputs):
     try:
         q = inputs['q']
         dq = inputs['dq']
         u = vectorops.div(vectorops.sub(inputs['qcmd'],q),inputs['dt'])
     except KeyError:
         print "Warning, cannot debug motion model, dq or dqcmd not in input"
         return None
     if self.dqpredlast != None:
         if self.activeDofs != None:
             dq = dq[:]
             for i in [i for i in range(len(q)) if i not in self.activeDofs]:
                 dq[i] = self.dqpredlast[i]
         #compare motion model to dq
         print "Motion model error:",np.linalg.norm(self.dqpredlast - np.array(dq))
         (v,i) = max(zip(np.abs(self.dqpredlast - np.array(dq)).tolist(),range(len(dq))))
         print "  Max error:",v,"at",i,
         if self.robot!=None: print self.robot.getLink(i).getName()
         else: print
         print "  Command:",self.ulast[i],"Predicted:",self.dqpredlast[i],"Actual:",dq[i]
         print "  pred:",self.Alast[i,i],"*u +",self.blast[i]
Exemple #26
0
def spawn_objects_from_ground_truth(world):
    """For all ground_truth_items, spawns RigidObjects in the world
    according to their sizes / mass properties"""

    print "Initializing world objects"
    for item in ground_truth_items:
        obj = world.makeRigidObject(item.info.name)
        bmin,bmax = item.info.bmin,item.info.bmax
        center = vectorops.div(vectorops.add(bmin,bmax),2.0)
        m = obj.getMass()
        m.setMass(item.info.mass)
        m.setCom([0,0,0])
        m.setInertia(vectorops.mul([bmax[0]-bmin[0],bmax[1]-bmin[1],bmax[2]-bmin[2]],item.info.mass/12.0))
        obj.setMass(m)
        c = obj.getContactParameters()
        c.kFriction = 0.6
        c.kRestitution = 0.1;
        c.kStiffness = 100000
        c.kDamping = 100000
        obj.setContactParameters(c)
        simgeometry = obj.geometry()
        load_item_geometry(item,simgeometry)
        obj.setTransform(item.xform[0],item.xform[1])
    return
def generate_trajectory(qi, qf):
    i = 0
    endIndex = 2
    path = [0.0, 0.0]
    path[0] = qi
    path[1] = qf
    print qi, qf
    while i < endIndex-1:
        # print i, endIndex
        q = path[i]
        qNext = path[i+1]
        dt = vectorops.distance(q,qNext)

        # smooth trajectory by interpolating between two consecutive configurations
        # if the distance between the two is big
        if dt>0.1:
            qInterp = vectorops.div(vectorops.add(q, qNext), 2)
            path.insert(i+1, qInterp)
            endIndex +=1
            continue
        else:
            i += 1
    print len(path)
    return path
        elif cmd == 'find':
            print master._find_object()

        elif cmd == 'grasp':
            print master._grasp_object()

        elif cmd == 'retrieve':
            print master._retrieve_object()

        elif cmd == 'move_cartesian' and len(args) == 4:
            task = None
            limb = args[0]
            amount = [float(args[1]),float(args[2]),float(args[3])]
            maxspeed = 0.01
            command = {'limb':limb,'velocity':vectorops.div(amount,vectorops.norm(amount)/maxspeed),'duration':vectorops.norm(amount)/maxspeed}
            print "Trying cartesian_drive low level command"
            task = master.manager.control.execute([
                ( 'cartesian_drive', command, 0, 0),
            ])

            while task and not task.done:
                sleep(0.1)

        elif cmd == 'gripper' and len(args) == 1:
            task = None

            if args[0] == 'parallel':
                task = master.manager.control.execute([
                    ( 'left_gripper', [0.5, 0.5, 0.5, 1], 0, 0),
                    ( 'right_gripper', [0.5, 0.5, 0.5, 1], 0, 0),
def sendPath(path,maxSmoothIters =0, INCREMENTAL=False, limb = None, readConstants=False, internalSpeed=SPEED):


    # interpolate path linearly between two endpoints
    if path == None:
        print "sending empty path"
        return False

    # n = self.robot.numLinks()
    # for i in range(len(path)):

    #     # if we specify a limb, and the path only has seven numbers (DOF for each arm, we shouldn't append 0's)
    #     if readConstants and limb is not None:
    #         # if we're reading a path from the milestones
    #         pass
    #     else:
            
    #         #print path
    #         if len(path[i])<n:
    #             path[i] += [0.0]*(n-len(path[i]))
    #         #pass
    for smoothIter in range(maxSmoothIters):
        # path = path
        smoothePath = [0]*(len(path)*2-1)
        for i in range(len(path)-1):
            smoothePath[i*2] = path[i]
            smoothePath[i*2 +1] = vectorops.div(vectorops.add(path[i],path[i+1]), 2)
        smoothePath[-1] = path[-1]
        path = smoothePath



    i = 0
    endIndex = len(path)            
    # if endIndex==1:
    #     q=path[0]
    #     path[0]=self.robot.getConfig()
    #     path.append(q)

    counter = 0

    #path.append(path[-1])
    #endIndex = len(path)

    #print 'myPath = ', path

    speed = 1
    while i+1 <endIndex:
        # print i, endIndex
        q = path[i]
        qNext = path[i+1]
        dt = vectorops.distance(q,qNext)
        dt = dt / speed

        i += 1
        waitForMove()
        # if INCREMENTAL:
        #     time.sleep(.1)
        if limb == 'left':
            CONTROLLER.appendMilestoneLeft(q,dt)
            #pass
        elif limb == 'right':
            CONTROLLER.appendMilestoneRight(q,dt)
            #pass
        else:
            #print 'milestone #', i, q
            #CONTROLLER.appendMilestone(q)
            CONTROLLER.appendLinear(q,dt)
            #pass


    """
    def loop(self):
        try:
            while True:
                print OKBLUE + "Bin " + str(self.current_bin) + ": " + self.state + END_COLOR

                if self.state == "VISUAL_DEBUG":
                    self.object_com = [1.0839953170961105, -0.25145094946424207, 1.1241831909823194]
                    # self.set_model_right_arm( [0.1868786927065213, -1.567604604679142, 0.6922768776941961, 1.5862815343628953, -0.005567750307534711, -0.017979599494945674, 0.0035268645585939083])
                    self.load_real_robot_state()
                else:
                    self.load_real_robot_state()

                self.Tcamera = se3.mul(
                    self.robotModel.link("right_lower_forearm").getTransform(), self.calibratedCameraXform
                )
                self.Tvacuum = se3.mul(self.robotModel.link("right_wrist").getTransform(), VACUUM_POINT_XFORM)

                self.vacuumPc = Geometry3D()
                self.vacuumPc.loadFile(VACUUM_PCD_FILE)
                temp_xform = self.robotModel.link("right_wrist").getTransform()
                self.vacuumPc.transform(self.Tvacuum[0], self.Tvacuum[1])

                if self.state == "DEBUG_COLLISION_CHECKER":
                    temp_planner = planning.LimbPlanner(self.world, self.vacuumPc)
                    print "Is this configuration collision free?"
                    print temp_planner.check_collision_free("right")
                    sys.stdout.flush()

                elif self.state == "FAKE_PATH_PLANNING":
                    self.object_com = [1.114, -0.077, 1.412]
                    self.right_arm_ik(self.object_com)
                    self.Tcamera = se3.mul(
                        self.robotModel.link("right_lower_forearm").getTransform(), self.calibratedCameraXform
                    )
                    self.Tvacuum = se3.mul(self.robotModel.link("right_wrist").getTransform(), VACUUM_POINT_XFORM)
                    time.sleep(2342340)

                elif self.state == "START":
                    self.state = "MOVE_TO_SCAN_BIN"

                elif self.state == "MOVE_TO_SCAN_BIN":
                    for milestone in eval("Q_BEFORE_SCAN_" + self.current_bin):
                        print "Moving to " + str(milestone)
                        motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(milestone))
                        while motion.robot.right_mq.moving():
                            time.sleep(1)
                        time.sleep(1)
                    motion.robot.right_mq.appendLinear(
                        MOVE_TIME, planning.cleanJointConfig(eval("Q_SCAN_BIN_" + self.current_bin))
                    )
                    self.state = "MOVING_TO_SCAN_BIN"

                elif self.state == "MOVING_TO_SCAN_BIN":
                    if not motion.robot.right_mq.moving():
                        self.wait_start_time = time.time()
                        self.state = "WAITING_TO_SCAN_BIN"

                elif self.state == "WAITING_TO_SCAN_BIN":
                    if time.time() - self.wait_start_time > SCAN_WAIT_TIME:
                        if REAL_PERCEPTION:
                            self.state = "SCANNING_BIN"
                        else:
                            self.state = "FAKE_SCANNING_BIN"

                elif self.state == "SCANNING_BIN":
                    print "Waiting for message from camera"
                    cloud = rospy.wait_for_message(ROS_DEPTH_TOPIC, PointCloud2)

                    if perception.isCloudValid(cloud):
                        cloud = perception.convertPc2ToNp(cloud)
                        np_cloud = cloud[::STEP]

                        self.points1 = []
                        for point in np_cloud:
                            transformed = se3.apply(self.Tcamera, point)
                            self.points1.append(transformed)

                        np_cloud = perception.subtractShelf(np_cloud, self.current_bin)

                        self.points2 = []
                        for point in np_cloud:
                            transformed = se3.apply(self.Tcamera, point)
                            self.points2.append(transformed)

                        self.object_com = se3.apply(self.Tcamera, perception.com(np_cloud))

                        if PRINT_BLOBS:
                            print np_cloud

                        sio.savemat(CLOUD_MAT_PATH, {"cloud": np_cloud})
                        fo = open(CHENYU_GO_PATH, "w")
                        fo.write("chenyugo")
                        fo.close()

                        self.object_com = se3.apply(self.Tcamera, perception.com(np_cloud))

                        if CALIBRATE_CAMERA:
                            self.state = "CALIBRATE_CAMERA"
                        elif SEGMENT:
                            self.state = "WAITING_FOR_SEGMENTATION"
                        else:
                            self.state = "MOVE_TO_GRASP_OBJECT"
                    else:
                        print FAIL_COLOR + "Got an invalid cloud, trying again" + END_COLOR

                elif self.state == "FAKE_SCANNING_BIN":
                    self.object_com = [1.1114839836097854, -0.6087936130127559, 0.9899267043340634]

                    if DRY_RUN:
                        raw_input("Hit enter to continue: ")

                    self.state = "MOVE_TO_GRASP_OBJECT"

                elif self.state == "CALIBRATE_CAMERA":
                    self.calibrateCamera()
                    self.state = "SCANNING_BIN"

                elif self.state == "WAITING_FOR_SEGMENTATION":
                    if os.path.isfile(CHENYU_DONE_PATH):
                        os.remove(CHENYU_DONE_PATH)
                        self.state = "POSTPROCESS_SEGMENTATION"

                elif self.state == "POSTPROCESS_SEGMENTATION":
                    object_blobs = []
                    time.sleep(5)
                    for i in range(1, 20):
                        seg_file_path = MAT_PATH + "seg" + str(i) + ".mat"
                        print seg_file_path
                        if os.path.isfile(seg_file_path):
                            print seg_file_path + " exists"
                            mat_contents = sio.loadmat(seg_file_path)
                            r = mat_contents["r"]
                            r = r[r[:, 1] != 0, :]
                            object_blobs.append(r)
                            os.remove(seg_file_path)
                    if PRINT_BLOBS:
                        print "============="
                        print "object blobs"
                        print object_blobs
                        print "============="

                    if len(object_blobs) == 0:
                        self.state = "BIN_DONE"
                    else:
                        object_list = [
                            ITEM_NUMBERS[item_str] for item_str in self.bin_state[self.current_bin]["contents"]
                        ]
                        target = ITEM_NUMBERS[self.bin_state[self.current_bin]["target"]]

                        histogram_dict = perception.loadHistogram(object_list)
                        cloud_label = {}  # key is the label of object, value is cloud points
                        label_score = {}  # key is the label, value is the current score for the object
                        for object_cloud in object_blobs:
                            if PRINT_BLOBS:
                                print "====================="
                                print "object_cloud"
                                print object_cloud
                                print "====================="
                            object_cloud = perception.resample(cloud, object_cloud, 3)
                            label, score = perception.objectMatch(object_cloud, histogram_dict)
                            if label in cloud_label:
                                if label_score[label] < score:
                                    label_score[label] = score
                                    cloud_label[label] = object_cloud
                            else:
                                cloud_label[label] = object_cloud
                                label_score[label] = score

                        if PRINT_LABELS_AND_SCORES:
                            print cloud_label
                            print "============="
                            print label_score
                        if target in cloud_label:
                            self.object_com = se3.apply(self.Tcamera, perception.com(cloud_label[target]))

                            self.points1 = []
                            for point in cloud_label[target]:
                                transformed = se3.apply(self.Tcamera, point)
                                self.points1.append(transformed)

                            self.points2 = []
                        else:
                            cloud_score = {}
                            histogram_dict = perception.loadHistogram([target])
                            for object_cloud in object_blobs:
                                object_cloud = perception.resample(cloud, object_cloud, 3)
                                label, score = perception.objectMatch(object_cloud, histogram_dict)
                                cloud_score[score] = object_cloud
                            sorted_cloud = sorted(cloud_score.items(), key=operator.itemgetter(0), reverse=True)
                            score = sorted_cloud[0][0]
                            com = perception.com(sorted_cloud[0][1])
                            self.points1 = []
                            self.points2 = []
                            for point in sorted_cloud[0][1]:
                                transformed = se3.apply(self.Tcamera, point)
                                self.points1.append(transformed)
                            self.object_com = se3.apply(self.Tcamera, com)
                        self.state = "MOVE_TO_GRASP_OBJECT"

                elif self.state == "MOVE_TO_GRASP_OBJECT":
                    for milestone in eval("Q_AFTER_SCAN_" + self.current_bin):
                        print "Moving to " + str(milestone)
                        motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(milestone))
                        while motion.robot.right_mq.moving():
                            time.sleep(1)
                        time.sleep(1)

                    motion.robot.right_mq.appendLinear(
                        MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
                    )
                    while motion.robot.right_mq.moving():
                        time.sleep(1)
                    time.sleep(1)

                    self.load_real_robot_state()
                    self.Tvacuum = se3.mul(self.robotModel.link("right_wrist").getTransform(), VACUUM_POINT_XFORM)

                    print WARNING_COLOR + str(self.object_com) + END_COLOR
                    self.object_com = vectorops.add(self.object_com, COM_ADJUSTMENT)

                    current_vacuum_point = se3.apply(self.Tvacuum, [0, 0, 0])
                    milestone = vectorops.add(current_vacuum_point, self.object_com)
                    milestone = vectorops.div(milestone, 2)

                    if DRY_RUN:
                        self.state = "MOVING_TO_GRASP_OBJECT"
                    else:
                        if self.right_arm_ik(milestone):
                            destination = self.robotModel.getConfig()
                            self.q_milestone = [destination[v] for v in self.right_arm_indices]
                            print WARNING_COLOR + "IK config for " + str(milestone) + ": " + str(
                                self.q_milestone
                            ) + END_COLOR
                            motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(self.q_milestone))
                        else:
                            print FAIL_COLOR + "Error: IK failed" + END_COLOR
                            sys.stdout.flush()
                            motion.robot.right_mq.appendLinear(
                                MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
                            )
                            while motion.robot.right_mq.moving():
                                time.sleep(1)
                            time.sleep(1)

                        while motion.robot.right_mq.moving():
                            time.sleep(1)
                        time.sleep(1)

                        if self.right_arm_ik(self.object_com):
                            destination = self.robotModel.getConfig()
                            print WARNING_COLOR + "IK config for " + str(self.object_com) + ": " + str(
                                [destination[v] for v in self.right_arm_indices]
                            ) + END_COLOR
                            motion.robot.right_mq.appendLinear(
                                MOVE_TIME, planning.cleanJointConfig([destination[v] for v in self.right_arm_indices])
                            )
                            self.state = "MOVING_TO_GRASP_OBJECT"
                        else:
                            print FAIL_COLOR + "Error: IK failed" + END_COLOR
                            sys.stdout.flush()
                            motion.robot.right_mq.appendLinear(
                                MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
                            )
                            while motion.robot.right_mq.moving():
                                time.sleep(1)
                            time.sleep(1)

                elif self.state == "MOVING_TO_GRASP_OBJECT":
                    if not motion.robot.right_mq.moving():
                        time.sleep(1)
                        self.state = "GRASP_OBJECT"

                elif self.state == "GRASP_OBJECT":
                    move_target = se3.apply(self.Tvacuum, [0, 0, 0])
                    move_target[2] = move_target[2] - GRASP_MOVE_DISTANCE - (MEAN_OBJ_HEIGHT / 2)
                    if self.right_arm_ik(move_target):
                        self.turnOnVacuum()
                        destination = self.robotModel.getConfig()
                        motion.robot.right_mq.appendLinear(
                            MOVE_TIME, planning.cleanJointConfig([destination[v] for v in self.right_arm_indices])
                        )
                    else:
                        print FAIL_COLOR + "Error: IK failed" + END_COLOR
                        sys.stdout.flush()
                        motion.robot.right_mq.appendLinear(
                            MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
                        )
                        while motion.robot.right_mq.moving():
                            time.sleep(1)
                        time.sleep(1)
                    self.wait_start_time = time.time()
                    self.state = "WAITING_TO_GRASP_OBJECT"

                elif self.state == "WAITING_TO_GRASP_OBJECT":
                    if time.time() - self.wait_start_time > GRASP_WAIT_TIME:
                        self.state = "MOVE_UP_BEFORE_RETRACT"

                elif self.state == "MOVE_UP_BEFORE_RETRACT":
                    move_target = se3.apply(self.Tvacuum, [0, 0, 0])
                    move_target[2] = move_target[2] + GRASP_MOVE_DISTANCE + (MEAN_OBJ_HEIGHT / 2)
                    if self.right_arm_ik(move_target):
                        self.turnOnVacuum()
                        destination = self.robotModel.getConfig()
                        motion.robot.right_mq.appendLinear(
                            MOVE_TIME, planning.cleanJointConfig([destination[v] for v in self.right_arm_indices])
                        )
                    else:
                        print FAIL_COLOR + "Error: IK failed" + END_COLOR
                        sys.stdout.flush()
                        motion.robot.right_mq.appendLinear(
                            MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
                        )
                        while motion.robot.right_mq.moving():
                            time.sleep(1)
                        time.sleep(1)
                    self.state = "MOVE_TO_STOW_OBJECT"

                elif self.state == "MOVE_TO_STOW_OBJECT":
                    motion.robot.right_mq.appendLinear(
                        MOVE_TIME, planning.cleanJointConfig(eval("Q_IK_SEED_" + self.current_bin))
                    )
                    while motion.robot.right_mq.moving():
                        time.sleep(1)
                    time.sleep(1)

                    if not DRY_RUN:
                        motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(self.q_milestone))
                        while motion.robot.right_mq.moving():
                            time.sleep(1)
                        time.sleep(1)

                    for milestone in eval("Q_AFTER_GRASP_" + self.current_bin):
                        print "Moving to " + str(milestone)
                        motion.robot.right_mq.appendLinear(MOVE_TIME, planning.cleanJointConfig(milestone))
                        while motion.robot.right_mq.moving():
                            time.sleep(1)
                        time.sleep(1)
                    motion.robot.right_mq.appendLinear(MOVE_TIME, Q_STOW)
                    self.state = "MOVING_TO_STOW_OBJECT"

                elif self.state == "MOVING_TO_STOW_OBJECT":
                    if not motion.robot.right_mq.moving():
                        self.state = "STOWING_OBJECT"

                elif self.state == "STOWING_OBJECT":
                    self.turnOffVacuum()
                    self.wait_start_time = time.time()
                    self.state = "WAITING_FOR_SECURE_STOW"

                elif self.state == "WAITING_FOR_SECURE_STOW":
                    if time.time() - self.wait_start_time > GRASP_WAIT_TIME:
                        self.state = "BIN_DONE" if SELECT_REAL_BIN else "DONE"

                elif self.state == "BIN_DONE":
                    self.bin_state[self.current_bin]["done"] = True
                    self.current_bin = planning.selectBin(self.bin_state)
                    if self.current_bin is None:
                        self.state = "DONE"
                    else:
                        self.state = "START"

                elif self.state == "DONE":
                    print "actual vacuum point: ", se3.apply(self.Tvacuum, [0, 0, 0])

                else:
                    print FAIL_COLOR + "Unknown state" + END_COLOR

                time.sleep(1)
        except KeyboardInterrupt:
            motion.robot.stopMotion()
            sys.exit(0)