Beispiel #1
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)
Beispiel #2
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)
Beispiel #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 += 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')
Beispiel #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 += 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')
 def onUpdate(self):
     global deviceState
     if len(deviceState)==0: return
     cobj = {'type':'multi_ik','safe':1,'components':[]}
     for i,dstate in enumerate(deviceState):
         if not dstate['buttonDown']: continue
         ikgoal = {}
         ikgoal['link'] = endEffectorIndices[i]
         Rlocal,tlocal = endEffectorLocalTransforms[i]
         ikgoal['localPosition'] = tlocal
         ikgoal['endPosition'] = dstate['widgetTransform'][1]
         ikgoal['endRotation'] = so3.moment(so3.mul(Rlocal,dstate['widgetTransform'][0]))
         ikgoal['posConstraint'] = 'fixed'
         ikgoal['rotConstraint'] = 'fixed'
         cobj['components'].append(ikgoal)
     if cobj != self.lastData:
         self.sendMessage({'type':'set','path':self.topic,'data':cobj})
         self.lastData = cobj
     return Service.onUpdate(self)
    def onMessage(self, msg):
        global deviceState, defaultDeviceState
        #print "Getting haptic message"
        #print msg
        if msg['type'] != 'MultiHapticState':
            print "Strange message type", msg['type']
            return
        if len(deviceState) == 0:
            print "Adding", len(
                msg['devices']) - len(deviceState), "haptic devices"
            while len(deviceState) < len(msg['devices']):
                deviceState.append(defaultDeviceState.copy())

        if len(msg['devices']) != len(deviceState):
            print "Incorrect number of devices in message:", len(
                msg['devices'])
            return

        #change overall state depending on button 2
        if 'events' in msg:
            for e in msg['events']:
                #print e['type']
                if e['type'] == 'b2_down':
                    dstate = deviceState[e['device']]
                    toggleMode(dstate)
                    print 'Changing device', e['device'], 'to state', dstate[
                        'mode']

        for i in range(len(deviceState)):
            dmsg = msg['devices'][i]
            dstate = deviceState[i]
            if dmsg['button1'] == 1:
                #drag widget if button 1 is down
                oldButtonDown = dstate['buttonDown']
                dstate['buttonDown'] = True
                #moving
                if dstate['mode'] == 'normal':
                    if not oldButtonDown:
                        dstate['moveStartDeviceTransform'] = (so3.from_moment(
                            dmsg['rotationMoment']), dmsg['position'])
                        if self.widgetGetters == None:
                            dstate['moveStartWidgetTransform'] = dstate[
                                'widgetTransform']
                        else:
                            Twidget = self.widgetGetters[i]()
                            if Twidget != None:
                                dstate['moveStartWidgetTransform'] = Twidget
                            else:
                                dstate['moveStartWidgetTransform'] = dstate[
                                    'widgetTransform']
#================================================================================================
#                    #perform transformation of widget
#                    deviceTransform = (so3.from_moment(dmsg['rotationMoment']),dmsg['position'])
#                    #compute relative motion
#                    Tdev = se3.mul(deviceToWidgetTransform,deviceTransform)
#                    TdevStart = se3.mul(deviceToWidgetTransform,dstate['moveStartDeviceTransform'])
#                    relDeviceTransform = (so3.mul(Tdev[0],so3.inv(TdevStart[0])),vectorops.sub(Tdev[1],TdevStart[1]))
#                    #scale relative motion
#                    Rrel,trel = relDeviceTransform
#                    wrel = so3.moment(Rrel)
#                    wrel = vectorops.mul(wrel,dstate['rotationScale'])
#                    trel = vectorops.mul(trel,dstate['positionScale'])
#                    #print "Moving",trel,"rotating",wrel
#                    relDeviceTransform = (so3.from_moment(wrel),trel)
#                    #perform transform
#                    dstate['widgetTransform'] = se3.mul(dstate['moveStartWidgetTransform'],relDeviceTransform)
#================================================================================================
#- perform transformation of widget
                    deviceTransform = (so3.from_moment(dmsg['rotationMoment']),
                                       dmsg['position'])
                    # delta_device = vectorops.sub(deviceTransform[1],dstate['moveStartDeviceTransform'][1])
                    # print "haptic moves: ", delta_device
                    delta_device_R_matrix = so3.mul(
                        deviceTransform[0],
                        so3.inv(dstate['moveStartDeviceTransform'][0]))
                    #                    delta_device_R = so3.moment(delta_device_R_matrix)
                    #                    norm_delta_R = vectorops.norm(delta_device_R)
                    #                    if norm_delta_R != 0:
                    #                        vec_delta_R = vectorops.div(delta_device_R, norm_delta_R)
                    #                    else:
                    #                        vec_delta_R = [0,0,0]
                    #
                    #                    print "haptic rotate: norm = ", norm_delta_R, ", vec = ", vec_delta_R

                    #- compute relative motion
                    Tdev = se3.mul(deviceToBaxterBaseTransform,
                                   deviceTransform)
                    TdevStart = se3.mul(deviceToBaxterBaseTransform,
                                        dstate['moveStartDeviceTransform'])
                    # delta_widget = vectorops.sub(Tdev[1],TdevStart[1])
                    # print "Baxter moves: ", delta_widget
                    #                    delta_widget_R_matrix = so3.mul(Tdev[0],so3.inv(TdevStart[0]))
                    #                    delta_widget_R = so3.moment(delta_widget_R_matrix)
                    #                    norm_widget_R = vectorops.norm(delta_widget_R)
                    #                    if norm_widget_R != 0:
                    #                        vec_widget_R = vectorops.div(delta_widget_R, norm_widget_R)
                    #                    else:
                    #                        vec_widget_R = [0,0,0]

                    #                    print "Baxter rotate: norm = ", norm_widget_R, ", vec = ", vec_widget_R

                    relDeviceTransform = (so3.mul(Tdev[0],
                                                  so3.inv(TdevStart[0])),
                                          vectorops.sub(Tdev[1], TdevStart[1]))

                    #- scale relative motion
                    Rrel, trel = relDeviceTransform
                    wrel = so3.moment(Rrel)
                    wrel = vectorops.mul(wrel, dstate['rotationScale'])
                    trel = vectorops.mul(trel, dstate['positionScale'])

                    #- print "Moving",trel,"rotating",wrel
                    relDeviceTransform = (so3.from_moment(wrel), trel)

                    #- perform transform
                    # dstate['widgetTransform'] = se3.mul(dstate['moveStartWidgetTransform'],relDeviceTransform)
                    dstate['widgetTransform'] = (
                        so3.mul(dstate['moveStartWidgetTransform'][0],
                                relDeviceTransform[0]),
                        vectorops.add(dstate['moveStartWidgetTransform'][1],
                                      relDeviceTransform[1]))


#================================================================================================

                elif dstate['mode'] == 'scaling':
                    if not oldButtonDown:
                        dstate['moveStartDeviceTransform'] = (so3.from_moment(
                            dmsg['rotationMoment']), dmsg['position'])
                    #perform scaling
                    deviceTransform = (so3.from_moment(dmsg['rotationMoment']),
                                       dmsg['position'])
                    oldDeviceTransform = dstate['moveStartDeviceTransform']
                    znew = deviceTransform[1][1]
                    zold = oldDeviceTransform[1][1]
                    posscalerange = [1e-2, 20]
                    rotscalerange = [0.5, 2]
                    newposscale = min(
                        max(
                            dstate['positionScale'] * math.exp(
                                (znew - zold) * 10), posscalerange[0]),
                        posscalerange[1])
                    newrotscale = min(
                        max(
                            dstate['rotationScale'] * math.exp(
                                (znew - zold) * 4), rotscalerange[0]),
                        rotscalerange[1])
                    if any(newposscale == v for v in posscalerange) or any(
                            newrotscale == v for v in rotscalerange):
                        pass  #dont change the scale
                    else:
                        dstate['positionScale'] = newposscale
                        dstate['rotationScale'] = newrotscale
                    print "Setting position scale to", dstate[
                        'positionScale'], "rotation scale to", dstate[
                            'rotationScale']
                    #update start device transform
                    dstate['moveStartDeviceTransform'] = deviceTransform
                elif dstate['mode'] == 'gripper':
                    print "Gripper mode not available"
                    dstate['mode'] = 'normal'
                    dstate['buttonDown'] = False
            else:
                dstate['buttonDown'] = False
        if self.viewer: self.viewer.update(deviceState)
        else: print "No viewer..."
    def move_to_grasp_object(self,object):
        """Sets the robot's configuration so the gripper grasps object at
        one of its potential grasp locations.  Might change self.active_limb
        to the appropriate limb.  Must change self.active_grasp to the
        selected grasp.

        If successful, sends the motion to the low-level controller and
        returns True.
        
        Otherwise, does not modify the low-level controller and returns False.
        """
        self.waitForMove()
        self.controller.commandGripper(self.active_limb,[1])
        grasps = self.knowledge.grasp_xforms(object)
        qmin,qmax = self.robot.getJointLimits()
        
        #get the end of the commandGripper motion
        qcmd = self.controller.getCommandedConfig()
        self.robot.setConfig(qcmd)
        baxter.set_model_gripper_command(self.robot,self.active_limb,[1])
        qcmd = self.robot.getConfig()
        
        #solve an ik solution to one of the grasps
        grasp_goals = []
        pregrasp_goals = []
        pregrasp_shift = [0,0,0.05]
        for (grasp,gxform) in grasps:
            if self.active_limb == 'left':
                Tg = se3.mul(gxform,se3.inv(baxter.left_gripper_center_xform))
                Tpg = se3.mul(gxform,se3.inv((baxter.left_gripper_center_xform[0],vectorops.add(baxter.left_gripper_center_xform[1],pregrasp_shift))))
                Tg = (so3.from_moment(so3.moment(Tg[0])),Tg[1])
                Tpg = (so3.from_moment(so3.moment(Tpg[0])),Tpg[1])
                goal = ik.objective(self.left_gripper_link,R=Tg[0],t=Tg[1])
                pregoal = ik.objective(self.left_gripper_link,R=Tpg[0],t=Tpg[1])
            else:
                Tg = se3.mul(gxform,se3.inv(baxter.right_gripper_center_xform))
                Tpg = se3.mul(gxform,se3.inv((baxter.right_gripper_center_xform[0],vectorops.add(baxter.right_gripper_center_xform[1],pregrasp_shift))))
                #hack: weird matrices coming from ODE aren't rotations within tolerance, gives abort
                Tg = (so3.from_moment(so3.moment(Tg[0])),Tg[1])
                Tpg = (so3.from_moment(so3.moment(Tpg[0])),Tpg[1])
                goal = ik.objective(self.right_gripper_link,R=Tg[0],t=Tg[1])
                pregoal = ik.objective(self.right_gripper_link,R=Tpg[0],t=Tpg[1])
            grasp_goals.append(goal)
            pregrasp_goals.append(pregoal)
        sortedSolutions = self.get_ik_solutions(pregrasp_goals,[self.active_limb]*len(grasp_goals),qcmd)
        if len(sortedSolutions)==0: return False

        #prototyping hack: move straight to target
        if SKIP_PATH_PLANNING:
            for pregrasp in sortedSolutions:
                graspIndex = pregrasp[1]
                gsolns = self.get_ik_solutions([grasp_goals[graspIndex]],[self.active_limb],pregrasp[0],maxResults=1)
                if len(gsolns)==0:
                    print "Couldn't find grasp config for pregrasp? Trying another"
                else:
                    self.waitForMove()
                    self.sendPath([pregrasp[0],gsolns[0][0]])
                    self.active_grasp = grasps[graspIndex][0]
                    return True
            print "Planning failed"
            return False
        for solution in sortedSolutions:
            path = self.planner.plan(qcmd,solution[0])
            graspIndex = solution[1]
            if path != None:
                #now solve for grasp
                gsolns = self.get_ik_solutions([grasp_goals[graspIndex]],[self.active_limb],path[-1],maxResults=1)
                if len(gsolns)==0:
                    print "Couldn't find grasp config??"
                else:
                    self.waitForMove()
                    self.sendPath(path+[gsolns[0][0]])
                    self.active_grasp = grasps[graspIndex][0]
                    return True
        print "Planning failed"
        return False