Example #1
0
def initWaistCoMTasks(robot):
  # ---- TASKS -------------------------------------------------------------------
  # Make sure that the CoM is not controlling the Z
  robot.featureCom.selec.value='011'

  # Build the reference waist pos h**o-matrix from PG.

  # extract yaw
  YawFromLeftRightRPY = Multiply_matrix_vector('YawFromLeftRightRPY')
  YawFromLeftRightRPY.sin1.value=matrixToTuple(array([[ 0.,  0.,  0.], \
       [ 0.,  0.,  0.,  ],
       [ 0.,  0.,  1.,  ]]))
  plug(robot.pg.signal('comattitude'),YawFromLeftRightRPY.sin2)

  # Build a reference vector from init waist pos and
  # init left foot roll pitch representation
  waistReferenceVector = Stack_of_vector('waistReferenceVector')
  plug(robot.pg.initwaistposref,waistReferenceVector.sin1)
  #plug(robot.pg.initwaistattref,waistReferenceVector.sin2)
  plug(YawFromLeftRightRPY.sout,waistReferenceVector.sin2)

  waistReferenceVector.selec1(0,3)
  waistReferenceVector.selec2(0,3)
  robot.pg.waistReference=PoseRollPitchYawToMatrixHomo('waistReference')

  # Controlling also the yaw.
  robot.waist.selec.value = '111100'

  robot.addTrace(robot.pg.waistReference.name,'sout')
  robot.addTrace(robot.geom.name,'position')
  robot.addTrace(robot.pg.name,'initwaistposref')
  plug(waistReferenceVector.sout, robot.pg.waistReference.sin)
  plug(robot.pg.waistReference.sout,robot.waist.reference)

  robot.tasks ['waist'].controlGain.value = 200
Example #2
0
def initPr2RosSimuProblem():
    robot = Pr2('pr2', device=RobotSimu('pr2'))
    plug(robot.device.state, robot.dynamic.position)
    ros = Ros(robot)
    sot = SotPr2(robot)
    [robot,sot] = initializePr2(robot,sot)
    return [robot,ros,sot]
def initChestTask(robot):
  name = "chest"
  dim = 1
  index = 18
  feature = FeatureGeneric('feature_'+name)
  featureDes = FeatureGeneric('featureDes_'+name)
  featureDes.errorIN.value=(0.2,)
  feature.setReference('featureDes_'+name)
  featureDes.errorIN.value = (0,) * dim;
  robot.features[name] = feature 
  robot.features['Des'+name] = feature 

  # create jacobian.
  jacobianGripper = eye(dim,robot.dimension) * 0;
  jacobianGripper[0][index] = 1;
  feature.jacobianIN.value = jacobianGripper

  # only selec some dofs
  selecFeatureChest = Selec_of_vector('selecfeature_'+name)
  selecFeatureChest.selec(index, index+dim)
  plug(robot.dynamic.position, selecFeatureChest.sin)
  plug(selecFeatureChest.sout, feature.errorIN)

  # 2\ Define the task. Associate to the task the position feature.
  task = Task('task_'+name)
  task.add('feature_'+name)
  task.controlGain.value = 1
  robot.tasks[name] = task
def test_ros(robot):
    # SCRIPT PARAMETERS
    j               = 0;        # index of joint under analysis
    N               = 300;      # test duration (in number of timesteps)
    dt              = 0.001;    # time step
    estimationDelay = 10;       # delay introduced by the estimation [number of time steps]
    createRosTopics = 1;        # 1=true, 0=false
    
    # CONSTANTS
    nj              = 30;       # number of joints
    rad2deg         = 180/3.14;

    app = Application(robot);
    dq_des = nj*(0.0,);
    
    app.robot.device.control.value = dq_des;

    if(createRosTopics==1):
        ros = RosExport('rosExport');
        ros.add('vector', 'robotStateRos', 'state');
        plug(robot.device.state, ros.robotStateRos);
        
#        robot.device.after.addSignal('rosExport.robotStateRos')
    
    return ros
    def __init__(self, name, signalPosition=None, signalJacobian = None,
                 referencePosition = None):
        self._feature = FeaturePoint6d(name)
        self.obj = self._feature.obj
        self._reference = FeaturePoint6d(name + '_ref')
        if referencePosition:
            self._reference.signal('position').value = tuple(referencePosition)
        if signalPosition:
            plug(signalPosition, self._feature.signal('position'))
        if signalJacobian:
            plug(signalJacobian, self._feature.signal('Jq'))
        self._feature.setReference(self._reference.name)
        self._feature.signal('selec').value = '111111'
        self._feature.frame('current')

        # Signals stored in members
        self.position = self._feature.signal('position')
        self.reference = self._reference.signal('position')
        self.velocity = self._reference.signal ('velocity')
        self.Jq = self._feature.signal('Jq')
        self.error = self._feature.signal('error')
        self.errordot = self._feature.signal ('errordot')
        self.selec = self._feature.signal('selec')

        self.signalMap = {'position':self.position,
                          'reference':self.reference,
                          'Jq':self.Jq,
                          'error':self.error,
                          'selec':self.selec}
    def start(self):
        """
        Initialize the motion capture system.

        Before calling this function, make sure:
        a. evart-to-client is launched and successfully track
          the body used to compute the error,
        b. the robot is placed at its starting position.
        """
        if len(self.corba.signals()) == 3:
            print ("evart-to-client not launched, abandon.")
            return False
        if len(self.corba.signal(self.localizationPerceivedBody).value) != 3:
            print ("waist not tracked, abandon.")
            return False

        self.sMm = self.computeWorldTransformationFromFoot()

        print("World transformation:")
        print(HomogeneousMatrixToXYZTheta(self.sMm))
        self.errorEstimator.setSensorToWorldTransformation(self.sMm)
        plug(self.corba.signal(self.localizationPerceivedBody),
             self.errorEstimator.position)
        plug(self.corba.signal(
                self.localizationPerceivedBody + 'Timestamp'),
             self.errorEstimator.positionTimestamp)
        print ("Initial error:")
        print (self.errorEstimator.error.value)
        return True
def createTasks(robot):
    
    # MetaTasks dictonary
    robot.mTasks = dict()
    robot.tasksIne = dict()

    # Foot contacts
    robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',
                                     robot.OperationalPointsMap['left-ankle'])
    robot.contactLF.feature.frame('desired')
    robot.contactLF.gain.setConstant(10)
    robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',
                                     robot.OperationalPointsMap['right-ankle'])
    robot.contactRF.feature.frame('desired')
    robot.contactRF.gain.setConstant(10)

    # MetaTasksKine6d for other operational points
    robot.mTasks['waist'] = MetaTaskKine6d('waist', robot.dynamic, 'waist', 
                                           robot.OperationalPointsMap['waist'])
    robot.mTasks['chest'] = MetaTaskKine6d('chest', robot.dynamic, 'chest', 
                                           robot.OperationalPointsMap['chest'])
    robot.mTasks['rh'] = MetaTaskKine6d('rh', robot.dynamic, 'rh', 
                                        robot.OperationalPointsMap['right-wrist'])
    robot.mTasks['lh'] = MetaTaskKine6d('lh', robot.dynamic, 'lh', 
                                        robot.OperationalPointsMap['left-wrist'])
    
    for taskName in robot.mTasks:
        robot.mTasks[taskName].feature.frame('desired')
        robot.mTasks[taskName].gain.setConstant(10)
   
    handMgrip=eye(4); handMgrip[0:3,3] = (0,0,-0.14)
    robot.mTasks['rh'].opmodif = matrixToTuple(handMgrip)
    robot.mTasks['lh'].opmodif = matrixToTuple(handMgrip)
 

    # CoM Task
    robot.mTasks['com'] = MetaTaskKineCom(robot.dynamic)
    robot.dynamic.com.recompute(0)
    robot.mTasks['com'].featureDes.errorIN.value = robot.dynamic.com.value
    robot.mTasks['com'].task.controlGain.value = 10
    robot.mTasks['com'].feature.selec.value = '011'

    # Posture Task
    robot.mTasks['posture'] = MetaTaskKinePosture(robot.dynamic)
    robot.mTasks['posture'].ref = robot.halfSitting
    robot.mTasks['posture'].gain.setConstant(5)   
    
    ## TASK INEQUALITY

    
    # Task Height
    featureHeight = FeatureGeneric('featureHeight')
    plug(robot.dynamic.com,featureHeight.errorIN)
    plug(robot.dynamic.Jcom,featureHeight.jacobianIN)
    robot.tasksIne['taskHeight']=TaskInequality('taskHeight')
    robot.tasksIne['taskHeight'].add(featureHeight.name)
    robot.tasksIne['taskHeight'].selec.value = '100'
    robot.tasksIne['taskHeight'].referenceInf.value = (0.,0.,0.)    # Xmin, Ymin
    robot.tasksIne['taskHeight'].referenceSup.value = (0.,0.,0.80771)  # Xmax, Ymax
    robot.tasksIne['taskHeight'].dt.value=robot.timeStep
Example #8
0
    def __init__(self, dyn, dt, name="lim"):
        self.dyn  = dyn
        self.name = name

        self.task = TaskDynLimits('task'+name)
        plug(dyn.position, self.task.position)
        plug(dyn.velocity, self.task.velocity)

        self.task.dt.value = dt
        self.task.controlGain.value = 10.0

        dyn.upperJl.recompute(0)
        dyn.lowerJl.recompute(0)
        self.task.referencePosInf.value = dyn.lowerJl.value
        self.task.referencePosSup.value = dyn.upperJl.value

        dqup = (0, 0, 0, 0, 0, 0, 
                200, 220, 250, 230, 290, 520,
                200, 220, 250, 230, 290, 520, 
                250, 140, 390, 390, 
                240, 140, 240, 130, 270, 180, 330, 
                240, 140, 240, 130, 270, 180, 330)
        dqup = (1000,)*len(dyn.upperJl.value)
        self.task.referenceVelInf.value = tuple([-val*pi/180 for val in dqup])
        self.task.referenceVelSup.value = tuple([ val*pi/180 for val in dqup])
def initPostureTask2(robot):
  robot.features['featurePosition'] = FeatureGeneric('featurePosition')
  robot.features['featurePositionDes'] = FeatureGeneric('featurePositionDes')
  robot.features['featurePosition'].setReference('featurePositionDes')
  plug(robot.dynamic.position,robot.features['featurePosition'].errorIN)
  robot.features['featurePositionDes'].errorIN.value = robot.halfSitting
  robot.features['featurePosition'].jacobianIN.value = totuple( identity(size(robot.dynamic.position.value)) )

  robot.tasks['robot_task_position'] = Task('robot_task_position')
  robot.tasks['robot_task_position'].add('featurePosition')

  #gainPosition = GainAdaptive('gainPosition')
  #gainPosition.set(0.1,0.1,125e3)
  #gainPosition.gain.value = 5
  #plug(robot.tasks['robot_task_position'].error,gainPosition.error)
  #plug(gainPosition.gain,robot.tasks['robot_task_position'].controlGain)
  robot.tasks['robot_task_position'].controlGain.value =2.

  #TODO: this is ??? specific.
  if robot.device.name == 'HRP2LAAS' or \
     robot.device.name == 'HRP2JRL':
    # reverse polish arms(1), head(1), chest(1), legs(0), waist(0)
    robot.features['featurePosition'].selec.value = \
      14*'1' + 2*'1' + 2*'1' + 12*'0' + 6*'0'
  elif robot.device.name == 'HRP4LIRMM':
    # reverse polish arms(1), head(1), chest(1), legs(0), waist(0)
    robot.features['featurePosition'].selec.value = \
      18*'1' + 2*'1' + 2*'1' + 12*'0' + 6*'0'
  elif robot.device.name == 'ROMEO':
    # reverse polish legs, arms, chest, waist
    robot.features['featurePosition'].selec.value = \
      14*'0' + 14*'1' + 5*'1' + 6*'0'
  else:
    robot.features['featurePosition'].selec.value = '1' * robot.dimension
    def __init__(self, motion, yamlData, defaultDirectories):
        checkDict('interval', yamlData)

        Motion.__init__(self, motion, yamlData)

        self.gain = yamlData.get('gain', 1.)
        self.name = yamlData['name']
        self.reference = yamlData['reference']

        self.task = Task('joint'+str(id(yamlData)))
        self.feature = FeaturePosture('jointFeaturePosture'+str(id(yamlData)))
        plug(motion.robot.device.state, self.feature.state)

        jointId = self.nameToId[self.name]

        posture = np.array(motion.robot.halfSitting)
        posture[jointId] = self.reference

        self.feature.setPosture(tuple(posture.tolist()))

        for i in xrange(len(posture) - 6):
            if i + 6 == jointId:
                self.feature.selectDof(i + 6, True)
            else:
                self.feature.selectDof(i + 6, False)

        self.task.add(self.feature.name)
        self.task.controlGain.value = self.gain

        # Push the task into supervisor.
        motion.supervisor.addTask(self.task.name,
                                  self.interval[0], self.interval[1],
                                  self.priority,
                                  (jointId,))
def setTaskGoal(robot, name, lower, upper, selec = '', gain = ()):
  if name in robot.tasks and name in robot.features:
    # Modification of the goal
    if lower != [] and upper != []:
      if lower == upper:
        robot.features[name].reference.value = lower
      else:
        robot.features[name].reference.value = (0,) *len(lower)
        robot.tasks[name].referenceInf.value = lower
        robot.tasks[name].referenceSup.value = upper

    # Fill the selec value
    if selec != '':
      robot.features[name].selec.value = selec

  # If the gain vector is provided, fill it.
  if name in robot.tasks:
    if gain != []:
      # damping factor for the gain
      coeff = 1
      if robot.tasks[name].className == 'TaskInequality':
        coeff = robot.tasks[name].dt.value

      if len(gain) == 1:
        robot.tasks[name].controlGain.value = gain[0]*coeff
      elif len(gain) == 3:
        gain = GainAdaptive('gain_'+name)
        gain.set(gain[0]*coeff,gain[1]*coeff,gain[2])
        plug(robot.tasks[name].error, gain.error)
        plug(gain.gain, robot.tasks[name].controlGain)
      # otherwise do not modify the gain

  else:
    print "task " + name + " does not exists"
    def initializeRobot(self):
        """
        If the robot model is correctly loaded, this method will then
        initialize the operational points, set the position to
        half-sitting with null velocity/acceleration.

        To finish, different tasks are initialized:
        - the center of mass task used to keep the robot stability
        - one task per operational point to ease robot control
        """
        if not self.dynamic:
            raise RunTimeError("robots models have to be initialized first")

        if not self.device:
            self.device = RobotSimu(self.name + "_device")

        # Freeflyer reference frame should be the same as global
        # frame so that operational point positions correspond to
        # position in freeflyer frame.
        self.device.set(self.halfSitting)
        plug(self.device.state, self.dynamic.position)

        if self.enableVelocityDerivator:
            self.velocityDerivator = Derivator_of_Vector("velocityDerivator")
            self.velocityDerivator.dt.value = self.timeStep
            plug(self.device.state, self.velocityDerivator.sin)
            plug(self.velocityDerivator.sout, self.dynamic.velocity)
        else:
            self.dynamic.velocity.value = self.dimension * (0.0,)

        if self.enableAccelerationDerivator:
            self.accelerationDerivator = Derivator_of_Vector("accelerationDerivator")
            self.accelerationDerivator.dt.value = self.timeStep
            plug(self.velocityDerivator.sout, self.accelerationDerivator.sin)
            plug(self.accelerationDerivator.sout, self.dynamic.acceleration)
        else:
            self.dynamic.acceleration.value = self.dimension * (0.0,)

        self.initializeOpPoints(self.dynamic)

        # --- additional frames ---
        self.frames = dict()
        frameName = "rightHand"
        self.frames[frameName] = self.createFrame(
            "{0}_{1}".format(self.name, frameName), self.dynamic.getHandParameter(True), "right-wrist"
        )
        # rightGripper is an alias for the rightHand:
        self.frames["rightGripper"] = self.frames[frameName]

        frameName = "leftHand"
        self.frames[frameName] = self.createFrame(
            "{0}_{1}".format(self.name, frameName), self.dynamic.getHandParameter(False), "left-wrist"
        )
        # leftGripper is an alias for the leftHand:
        self.frames["leftGripper"] = self.frames[frameName]

        for (frameName, transformation, signalName) in self.AdditionalFrames:
            self.frames[frameName] = self.createFrame(
                "{0}_{1}".format(self.name, frameName), transformation, signalName
            )
def main(dt=0.001, delay=0.01):
    np.set_printoptions(precision=3, suppress=True);
    dt = conf.dt;
    q0 = conf.q0_urdf;
    v0 = conf.v0;
    nv = v0.shape[0];
    
    simulator  = Simulator('hrp2_sim', q0, v0, conf.fMin, conf.mu, dt, conf.model_path, conf.urdfFileName);
    simulator.ENABLE_TORQUE_LIMITS = conf.FORCE_TORQUE_LIMITS;
    simulator.ENABLE_FORCE_LIMITS = conf.ENABLE_FORCE_LIMITS;
    simulator.ENABLE_JOINT_LIMITS = conf.FORCE_JOINT_LIMITS;
    simulator.ACCOUNT_FOR_ROTOR_INERTIAS = conf.ACCOUNT_FOR_ROTOR_INERTIAS;
    simulator.VIEWER_DT = conf.DT_VIEWER;
    simulator.CONTACT_FORCE_ARROW_SCALE = 2e-3;
    simulator.verb=0;
    robot = simulator.r;
    
    q0 = np.array(conf.q0_sot);
#    q0[8] -= 1.0;
    device          = create_device(tuple(q0));

    estimator = ForceTorqueEstimator("estimator");
    plug(device.state,     estimator.base6d_encoders);
    estimator.accelerometer.value = (0.0, 0.0, 9.81);
    estimator.gyroscope.value = (0.0, 0.0, 0.0);
    estimator.ftSensRightFoot.value = 6*(0.0,);
    estimator.ftSensLeftFoot.value  = 6*(0.0,);
    estimator.ftSensRightHand.value = 6*(0.0,);
    estimator.ftSensLeftHand.value  = 6*(0.0,);
    estimator.currentMeasure.value = NJ*(0.0,);
    estimator.dqRef.value = NJ*(0.0,);
    estimator.ddqRef.value = NJ*(0.0,);
    estimator.wCurrentTrust.value     = tuple(NJ*[0.0,])
    estimator.saturationCurrent.value = tuple(NJ*[5.0,])
    estimator.motorParameterKt_p.value  = tuple(Kt_p)
    estimator.motorParameterKt_n.value  = tuple(Kt_n)
    estimator.motorParameterKf_p.value  = tuple(Kf_p)
    estimator.motorParameterKf_n.value  = tuple(Kf_n)
    estimator.motorParameterKv_p.value  = tuple(Kv_p)
    estimator.motorParameterKv_n.value  = tuple(Kv_n)
    estimator.motorParameterKa_p.value  = tuple(Ka_p)
    estimator.motorParameterKa_n.value  = tuple(Ka_n)
    estimator.init(dt,delay,delay,delay,delay,delay,True);
    estimator.setUseRawEncoders(True);
    estimator.setUseRefJointVel(True);
    estimator.setUseRefJointAcc(True);
    estimator.setUseFTsensors(False);

    estimator.jointsTorques.recompute(0);
    device.increment(dt);
        
    q = np.matrix(device.state.value).T;
    q_urdf = config_sot_to_urdf(q)
    simulator.viewer.updateRobotConfig(q_urdf);

    print "tau estimator=", np.array(estimator.jointsTorques.value);
    print "tau pinocchio=", robot.bias(q_urdf, conf.v0)[6:,0].T;
    
    return (simulator, estimator);
Example #14
0
def initRobotGeom(robot):
  robot.geom.createOpPoint('rf2',robot.OperationalPointsMap['right-ankle'])
  robot.geom.createOpPoint('lf2',robot.OperationalPointsMap['left-ankle'])
  plug(robot.dynamic.position,robot.geom.position)
  robot.geom.ffposition.value = 6*(0,)
  robotDim = len(robot.dynamic.velocity.value)
  robot.geom.velocity.value = robotDim * (0,)
  robot.geom.acceleration.value = robotDim * (0,)
    def __init__(self, robot):
        self.robot = robot
        self.ros = RosExport("rosExportMocap")
        self.ros.add("matrixHomoStamped", "chest", "/evart/hrp2_head_sf/hrp2_head_sf")

        self.mocapFilter = MocapDataFilter("MocapDataFilter")
        plug(self.ros.signal("chest"), self.mocapFilter.sin)
        self.mocapSignal = self.mocapFilter.sout
 def plugEverything(self):
     self.feature.setReference(self.featureDes.name)
     plug(self.dyn.signal(self.opPoint),self.feature.signal('position'))
     plug(self.dyn.signal('J'+self.opPoint),self.feature.signal('Jq'))
     self.task.add(self.feature.name)
     plug(self.dyn.velocity,self.task.qdot)
     plug(self.task.error,self.gain.error)
     plug(self.gain.gain,self.task.controlGain)
 def createFrame(self, frameName, transformation, operationalPoint):
     frame = OpPointModifier(frameName)
     frame.setTransformation(transformation)
     plug(self.dynamic.signal(operationalPoint), frame.positionIN)
     plug(self.dynamic.signal("J{0}".format(operationalPoint)), frame.jacobianIN)
     frame.position.recompute(frame.position.time + 1)
     frame.jacobian.recompute(frame.jacobian.time + 1)
     return frame
def createBalanceTask (robot, application, taskName, ingain = 1.):
    task = Task (taskName)
    task.add (application.featureCom.name)
    task.add (application.leftAnkle.name)
    task.add (application.rightAnkle.name)
    gain = GainAdaptive('gain'+taskName)
    gain.setConstant(ingain)
    plug(gain.gain, task.controlGain)
    plug(task.error, gain.error)      
    return (task, gain)
 def __init__(self,dyn,name="posture"):
     self.dyn=dyn
     self.name=name
     self.feature    = FeatureGeneric('feature'+name)
     self.featureDes = FeatureGeneric('featureDes'+name)
     self.gain = GainAdaptive('gain'+name)
     plug(dyn.position,self.feature.errorIN)
     robotDim = len(dyn.position.value)
     self.feature.jacobianIN.value = matrixToTuple( identity(robotDim) )
     self.feature.setReference(self.featureDes.name)
    def __init__(self, robot):
        self.robot = robot
        self.sot = SOT('solver')
        self.sot.signal('damping').value = 1e-6
        self.sot.setNumberDofs(self.robot.dimension)

        if robot.simu:
            plug(self.sot.signal('control'), robot.simu.signal('control'))
            plug(self.robot.simu.state,
                 self.robot.dynamic.position)
Example #21
0
 def createTrunkTask (self, robot, taskName, ingain=1.0):
     task = Task (taskName)
     task.add (self.features['chest'].name)
     task.add (self.features['waist'].name)
     task.add (self.features['gaze'].name)
     gain = GainAdaptive(taskName+'gain')
     gain.setConstant(ingain)
     plug(gain.gain, task.controlGain)
     plug(task.error, gain.error)
     return (task, gain)
def createRosExport(type, exportSignal, topic):
    # get global ros instance  
    global exportcounter
    exportcounter += 1
    rosExportSignalIntern = 'rosExportIntern'+str(exportcounter)
    ros.rosExport.add(type, rosExportSignalIntern, topic)
    # cannot be initialized empty due to serialization errors
    if (type == 'Vector'):
        ros.rosExport.rosExportSignalIntern = (100000,)
    plug(ros.rosExport.signal(rosExportSignalIntern), exportSignal)
def estimateCupFrameInHand(robot):
  # consider the difference of position between the bottle and the hand.
  # starting from that, define the robot (frame) corresponding to the top
  # of the bottle.
  topcupFrame = OpPointModifier('topcup')
  #TODO find the correct Z, position of the topcup in the frame of the hand
  topcupFrame.setTransformation(((1,0,0,0.0), (0,1,0,0.0), (0,0,1,-0.10), (0,0,0,1)))
  plug(robot.frames['leftGripper'].position, topcupFrame.positionIN)
  plug(robot.frames['leftGripper'].jacobian, topcupFrame.jacobianIN)
  robot.frames['topcup'] = topcupFrame
Example #24
0
def Pr2JointLimitsTask(robot,dt):
    robot.dynamic.upperJl.recompute(0)
    robot.dynamic.lowerJl.recompute(0)
    task = TaskJointLimits('taskJL')
    plug(robot.dynamic.position,task.position)
    task.controlGain.value = 10
    task.referenceInf.value = robot.dynamic.lowerJl.value
    task.referenceSup.value = robot.dynamic.upperJl.value
    task.dt.value = dt
    task.selec.value = toFlags(range(18,25)+range(26,27)+range(28,31)+range(32,40)+range(41,42)+range(43,46)+range(47,50))
    return task
 def plugComTask(self, taskCom):
     plug(self.comRef.ref, taskCom.featureDes.errorIN)
     plug(self.pg.dcomref, taskCom.featureDes.errordotIN)
     taskCom.task = TaskPD("taskComPD")
     taskCom.task.add(taskCom.feature.name)
     # This next line is not very nice. The principle should be reported in Task.
     plug(taskCom.feature.errordot, taskCom.task.errorDot)
     plug(taskCom.task.error, taskCom.gain.error)
     plug(taskCom.gain.gain, taskCom.task.controlGain)
     taskCom.gain.setConstant(40)
     taskCom.task.setBeta(-1)
 def createTask(self,diag,gain,sampleInterval):
     self.task = TaskJointWeights(self.name)
     if(diag):
         self.task.setWeights(diag)
     else:
         self.dyn.signal('inertia').recompute(0)
         plug(self.dyn.signal('inertia'),self.task.signal('weights'))
     #plug(self.dyn.position,self.task.velocity)
     plug(self.robot.device.controlOut,self.task.velocity)
     self.task.controlGain.value = gain
     self.task.setSampleInterval(sampleInterval)
 def createTask(self,selec,diag,gain,sampleInterval):
     self.task = TaskJointWeights(self.name)
     self.task.setSize(self.robot.dimension)
     self.task.selec.value = selec
     if(diag):
         self.task.setWeights(diag)
     else:
         self.dyn.signal('inertia').recompute(0)
         plug(self.dyn.signal('inertia'),self.task.signal('weights'))
     self.task.controlGain.value = gain
     self.task.setSampleInterval(sampleInterval)
Example #28
0
def initializePr2(robot,sot):
    robot.dynamic.velocity.value = robot.dimension*(0.,)
    robot.dynamic.acceleration.value = robot.dimension*(0.,)
    robot.dynamic.ffposition.unplug()
    robot.dynamic.ffvelocity.unplug()
    robot.dynamic.ffacceleration.unplug()
    robot.dynamic.setProperty('ComputeBackwardDynamics','true')
    robot.dynamic.setProperty('ComputeAccelerationCoM','true')
    robot.device.control.unplug()
    plug(sot.control,robot.device.control)
    return [robot,sot]
Example #29
0
def initFeetTask(robot):
  robot.selecFeet = Selector('selecFeet',
                             ['matrixHomo','leftfootref', \
                               robot.dynamic.signal(robot.OperationalPointsMap['left-ankle']),\
                               robot.pg.leftfootref], \
                             ['matrixHomo','rightfootref', \
                              robot.dynamic.signal(robot.OperationalPointsMap['right-ankle']), \
                              robot.pg.rightfootref])

  plug(robot.pg.inprocess,robot.selecFeet.selec)
  robot.tasks['right-ankle'].controlGain.value = 200
  robot.tasks['left-ankle'].controlGain.value = 200
def estimateBottleFrameInHand(robot):
  # consider the difference of position between the bottle and the hand.
  # starting from that, define the robot (frame) corresponding to the top
  # of the bottle.
  bungFrame = OpPointModifier('bung')
  #TODO find the correct Z, position of the bung in the frame of the hand
  bungFrame.setTransformation(((1,0,0,0.0), (0,1,0,0.0), (0,0,1, 0.10), (0,0,0,1)))
  plug(robot.frames['rightGripper'].position, bungFrame.positionIN)
  plug(robot.frames['rightGripper'].jacobian, bungFrame.jacobianIN)
#  frame.position.recompute(bungFrame.position.time + 1)
#  frame.jacobian.recompute(bungFrame.jacobian.time + 1)
  robot.frames['bung'] = bungFrame
def create_base_estimator(robot, dt, conf, robot_name="robot"):    
    from dynamic_graph.sot.torque_control.base_estimator import BaseEstimator
    base_estimator = BaseEstimator('base_estimator');
    plug(robot.encoders.sout,               base_estimator.joint_positions);
    plug(robot.device.forceRLEG,            base_estimator.forceRLEG);
    plug(robot.device.forceLLEG,            base_estimator.forceLLEG);
    plug(robot.estimator_kin.dx,            base_estimator.joint_velocities);
    plug(robot.imu_filter.imu_quat,         base_estimator.imu_quaternion);
    plug(robot.imu_offset_compensation.accelerometer_out, base_estimator.accelerometer);
    
    base_estimator.K_fb_feet_poses.value = conf.K_fb_feet_poses;
    
    base_estimator.set_imu_weight(conf.w_imu);
    base_estimator.set_stiffness_right_foot(conf.K);
    base_estimator.set_stiffness_left_foot(conf.K);
    base_estimator.set_zmp_std_dev_right_foot(conf.std_dev_zmp)
    base_estimator.set_zmp_std_dev_left_foot(conf.std_dev_zmp)
    base_estimator.set_normal_force_std_dev_right_foot(conf.std_dev_fz)
    base_estimator.set_normal_force_std_dev_left_foot(conf.std_dev_fz)
    base_estimator.set_zmp_margin_right_foot(conf.zmp_margin)
    base_estimator.set_zmp_margin_left_foot(conf.zmp_margin)
    base_estimator.set_normal_force_margin_right_foot(conf.normal_force_margin)
    base_estimator.set_normal_force_margin_left_foot(conf.normal_force_margin)
    base_estimator.set_right_foot_sizes(conf.RIGHT_FOOT_SIZES)
    base_estimator.set_left_foot_sizes(conf.LEFT_FOOT_SIZES)
    
    base_estimator.init(dt, robot_name);
    return base_estimator;
def create_admittance_ctrl(robot, dt=0.001):
    admit_ctrl = AdmittanceController("adm_ctrl");
    plug(robot.device.robotState,             admit_ctrl.base6d_encoders);
    plug(robot.estimator_kin.dx,    admit_ctrl.jointsVelocities);
    plug(robot.estimator_ft.contactWrenchRightSole,   admit_ctrl.fRightFoot);
    plug(robot.estimator_ft.contactWrenchLeftSole,    admit_ctrl.fLeftFoot);
    plug(robot.estimator_ft.contactWrenchRightHand,   admit_ctrl.fRightHand);
    plug(robot.estimator_ft.contactWrenchLeftHand,    admit_ctrl.fLeftHand);
    plug(robot.traj_gen.fRightFoot,           admit_ctrl.fRightFootRef);
    plug(robot.traj_gen.fLeftFoot,            admit_ctrl.fLeftFootRef);
    plug(robot.traj_gen.fRightHand,           admit_ctrl.fRightHandRef);
    plug(robot.traj_gen.fLeftHand,            admit_ctrl.fLeftHandRef);
    
    admit_ctrl.damping.value = 4*(0.05,);
    admit_ctrl.Kd.value = NJ*(0,);
    kf = -0.0005;
    km = -0.008;
    admit_ctrl.Kf.value = 3*(kf,)+3*(km,)+3*(kf,)+3*(km,)+3*(kf,)+3*(km,)+3*(kf,)+3*(km,);
    
    robot.ctrl_manager.addCtrlMode("adm");
    plug(admit_ctrl.qDes,                       robot.ctrl_manager.ctrl_adm);
    plug(robot.ctrl_manager.joints_ctrl_mode_adm, admit_ctrl.controlledJoints);
    
    admit_ctrl.init(dt);
    return admit_ctrl;
def create_topic(ros_import, signal, name, data_type='vector', sleep_time=0.1):
    ros_import.add(data_type, name+'_ros', name);
    plug(signal, ros_import.signal(name+'_ros'));
    from time import sleep
    sleep(sleep_time);
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g / h)

# -------------------------- ESTIMATION --------------------------

# --- Base Estimation
robot.param_server = create_parameter_server(param_server_conf, dt)
robot.device_filters = create_device_filters(robot, dt)
robot.imu_filters = create_imu_filters(robot, dt)
robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
# robot.be_filters              = create_be_filters(robot, dt)

# --- Conversion
e2q = EulerToQuat('e2q')
plug(robot.base_estimator.q, e2q.euler)
robot.e2q = e2q

# --- Kinematic computations
robot.rdynamic = DynamicPinocchio("real_dynamics")
robot.rdynamic.setModel(robot.dynamic.model)
robot.rdynamic.setData(robot.rdynamic.model.createData())
plug(robot.base_estimator.q, robot.rdynamic.position)
robot.rdynamic.velocity.value = [0.0] * robotDim
robot.rdynamic.acceleration.value = [0.0] * robotDim

# --- CoM Estimation
cdc_estimator = DcmEstimator('cdc_estimator')
cdc_estimator.init(dt, robot_name)
plug(robot.e2q.quaternion, cdc_estimator.q)
plug(robot.base_estimator.v, cdc_estimator.v)
Example #35
0
dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
dyn.parse()

dyn.lowerJl.recompute(0)
dyn.upperJl.recompute(0)
llimit = matrix(dyn.lowerJl.value)
ulimit = matrix(dyn.upperJl.value)
dlimit = ulimit-llimit
mlimit = (ulimit+llimit)/2
llimit[6:18] = mlimit[6:12] - dlimit[6:12]*0.48
dyn.upperJl.value = vectorToTuple(ulimit)

dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value    = gearRatio[robotName]

plug(robot.state,dyn.position)
plug(robot.velocity,dyn.velocity)
dyn.acceleration.value = robotDim*(0.,)

dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()

dyn.setProperty('ComputeBackwardDynamics','true')
dyn.setProperty('ComputeAccelerationCoM','true')

robot.control.unplug()



#-----------------------------------------------------------------------------
    def __init__(self,
                 name,
                 basePosition=None,
                 otherPosition=None,
                 baseReference=None,
                 otherReference=None,
                 JqBase=None,
                 JqOther=None):
        self._feature = FeaturePoint6dRelative(name)
        self.obj = self._feature.obj
        self._reference = FeaturePoint6dRelative(name + '_ref')
        # Set undefined input parameters as identity matrix
        if basePosition is None:
            basePosition = I4
        if otherPosition is None:
            otherPosition = I4
        if baseReference is None:
            baseReference = I4
        if otherReference is None:
            otherReference = I4

        # If input positions are signals, plug them, otherwise set values
        for (sout, sin) in \
            ((basePosition, self._feature.signal('positionRef')),
             (otherPosition, self._feature.signal('position')),
             (baseReference, self._reference.signal('positionRef')),
             (otherReference, self._reference.signal('position'))):
            if isinstance(sout, SignalBase):
                plug(sout, sin)
            else:
                sin.value = sout

        if JqBase:
            plug(JqBase, self._feature.signal('JqRef'))
        if JqOther:
            plug(JqOther, self._feature.signal('Jq'))
        self._feature.setReference(self._reference.name)
        self._feature.signal('selec').value = Flags('111111')
        self._feature.frame('current')

        # Signals stored in members
        self.basePosition = self._feature.signal('positionRef')
        self.otherPosition = self._feature.signal('position')
        self.baseReference = self._reference.signal('positionRef')
        self.otherReference = self._reference.signal('position')
        self.JqBase = self._feature.signal('JqRef')
        self.JqOther = self._feature.signal('Jq')
        self.error = self._feature.signal('error')
        self.jacobian = self._feature.signal('jacobian')
        self.selec = self._feature.signal('selec')

        self.signalMap = {
            'basePosition': self.basePosition,
            'otherPosition': self.otherPosition,
            'baseReference': self.baseReference,
            'otherReference': self.otherReference,
            'JqBase': self.JqBase,
            'JqOther': self.JqOther,
            'error': self.error,
            'jacobian': self.jacobian,
            'selec': self.selec
        }
Example #37
0

@optionalparentheses
def dump():
    history.dumpToOpenHRP(options.output)


# --- DYN ----------------------------------------------------------------------
dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName], specificitiesPath, jointRankPath)
dyn.parse()

dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]

plug(robot.state, dyn.position)
dyn.velocity.value = robotDim * (0., )
dyn.acceleration.value = robotDim * (0., )

dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()
robot.control.unplug()

# --- META TASKS AND OP POINT ------------------------------------------------

taskRF = MetaTask6d('rf', dyn, 'rf', 'right-ankle')
taskLF = MetaTask6d('lf', dyn, 'lf', 'left-ankle')

# --- PG ---------------------------------------------------------
from dynamic_graph.sot.pattern_generator import PatternGenerator, Selector
Example #38
0
#-----------------------------------------------------------------------------

modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'

robot.dynamic = Dynamic("dyn")
robot.dynamic.setFiles(modelDir, modelName[robotName], specificitiesPath,
                       jointRankPath)
robot.dynamic.parse()

robot.dynamic.inertiaRotor.value = inertiaRotor[robotName]
robot.dynamic.gearRatio.value = gearRatio[robotName]

plug(robot.device.state, robot.dynamic.position)
robot.dynamic.velocity.value = robot.dimension * (0., )
robot.dynamic.acceleration.value = robot.dimension * (0., )

robot.dynamic.ffposition.unplug()
robot.dynamic.ffvelocity.unplug()
robot.dynamic.ffacceleration.unplug()

robot.dynamic.setProperty('ComputeBackwardDynamics', 'true')
robot.dynamic.setProperty('ComputeAccelerationCoM', 'true')

robot.device.control.unplug()

# ------------------------------------------------------------------------------
# --- SOLVER ----------------------------------------------------------------
# ------------------------------------------------------------------------------
Example #39
0
taskCom.feature.selec.value = '011'

# --- POSTURE
robot.taskUpperBody = Task('task_upper_body')
robot.taskUpperBody.feature = FeaturePosture('feature_upper_body')

q = list(robot.dynamic.position.value)
robot.taskUpperBody.feature.state.value = q
robot.taskUpperBody.feature.posture.value = q

robot.taskUpperBody.feature.selectDof(27, True)
robot.taskUpperBody.feature.selectDof(35, True)

robot.taskUpperBody.controlGain.value = 100.0
robot.taskUpperBody.add(robot.taskUpperBody.feature.name)
plug(robot.dynamic.position, robot.taskUpperBody.feature.state)

# --- CONTACTS (define contactLF and contactRF)
contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
                           robot.OperationalPointsMap['left-ankle'])
contactLF.feature.frame('desired')
contactLF.gain.setConstant(10)
contactLF.keep()
locals()['contactLF'] = contactLF

contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
                           robot.OperationalPointsMap['right-ankle'])
contactRF.feature.frame('desired')
contactRF.gain.setConstant(10)
contactRF.keep()
locals()['contactRF'] = contactRF
Example #40
0
# --- DYN ----------------------------------------------------------------------
# --- DYN ----------------------------------------------------------------------

modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'

dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName],specificitiesPath,jointRankPath)
dyn.parse()

dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]

plug(robot.state,dyn.position)
dyn.velocity.value = robotDim*(0.,)
dyn.acceleration.value = robotDim*(0.,)

dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()

#dyn.setProperty('ComputeBackwardDynamics','true')
#dyn.setProperty('ComputeAccelerationCoM','true')

robot.control.unplug()

# --- Task Dyn -----------------------------------------
class MetaTaskKine6d( MetaTask6d ):
    def createTask(self):
pinocchioRobot.display(fromSotToPinocchio(initialConfig))

from dynamic_graph.sot.dynamics_pinocchio.humanoid_robot import HumanoidRobot

robot = HumanoidRobot(robotName, pinocchioRobot.model, pinocchioRobot.data,
                      initialConfig, OperationalPointsMap)

# ------------------------------------------------------------------------------
# ---- Kinematic Stack of Tasks (SoT)  -----------------------------------------
# ------------------------------------------------------------------------------
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT

sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())
plug(sot.control, robot.device.control)

# ------------------------------------------------------------------------------
# ---- TASKS -------------------------------------------------------------------

# ---- TASK GRIP
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import eye

taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh',
                        robot.OperationalPointsMap['right-wrist'])
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')
Example #42
0
taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh',
                        robot.OperationalPointsMap['wrist'])
handMgrip = eye(4)
handMgrip[0:3, 3] = (0.1, 0, 0)
taskRH.opmodif = matrixToTuple(handMgrip)
taskRH.feature.frame('desired')

projection = HolonomicProjection("projection")
projection.setSize(robot.dynamic.getDimension())
projection.setLeftWheel(6)
projection.setRightWheel(7)
# The wheel separation could be obtained with pinocchio.
# See pmb2_description/urdf/base.urdf.xacro
projection.setWheelRadius(0.0985)
projection.setWheelSeparation(0.4044)
plug(robot.dynamic.mobilebase, projection.basePose)

sot = SOT('sot')
sot.setSize(robot.dynamic.getDimension())

plug(projection.projection, sot.proj0)

plug(sot.control, robot.device.control)

ros_publish_state = RosPublish("ros_publish_state")
ros_publish_state.add("vector", "state", "/sot_control/state")
plug(robot.device.state, ros_publish_state.state)
robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100)

target = (0.5, -0.2, 1.0)
gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9))
Example #43
0
#-----------------------------------------------------------------------------
#---- DYN --------------------------------------------------------------------
#-----------------------------------------------------------------------------
modelDir = pkgDataRootDir[robotName]
xmlDir = pkgDataRootDir[robotName]
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'

dyn = Dynamic("dyn")
dyn.setFiles(modelDir, modelName[robotName], specificitiesPath, jointRankPath)
dyn.parse()

dyn.inertiaRotor.value = inertiaRotor[robotName]
dyn.gearRatio.value = gearRatio[robotName]

plug(robot.state, dyn.position)
dyn.velocity.value = robotDim * (0., )
dyn.acceleration.value = robotDim * (0., )

dyn.ffposition.unplug()
dyn.ffvelocity.unplug()
dyn.ffacceleration.unplug()

dyn.setProperty('ComputeBackwardDynamics', 'true')
dyn.setProperty('ComputeAccelerationCoM', 'true')

robot.control.unplug()

# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
# ---- SOT ---------------------------------------------------------------------
Example #44
0
    def __init__(self, dyn):
        self.pg = PatternGenerator('pg')

        modelDir = dyn.getProperty('vrmlDirectory')
        modelName = dyn.getProperty('vrmlMainFile')
        specificitiesPath = dyn.getProperty('xmlSpecificityFile')
        jointRankPath = dyn.getProperty('xmlRankFile')
        robotDim = len(dyn.position.value)
        # print(modelDir,modelName,specificitiesPath,jointRankPath,robotDim)

        self.pg.setVrmlDir(modelDir + '/')
        self.pg.setVrml(modelName)
        self.pg.setXmlSpec(specificitiesPath)
        self.pg.setXmlRank(jointRankPath)
        self.pg.buildModel()

        # Standard initialization
        self.pg.parseCmd(":samplingperiod 0.005")
        self.pg.parseCmd(":previewcontroltime 1.6")
        self.pg.parseCmd(":comheight 0.814")
        self.pg.parseCmd(":omega 0.0")
        self.pg.parseCmd(":stepheight 0.05")
        self.pg.parseCmd(":singlesupporttime 0.780")
        self.pg.parseCmd(":doublesupporttime 0.020")
        self.pg.parseCmd(":armparameters 0.5")
        self.pg.parseCmd(":LimitsFeasibility 0.0")
        self.pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
        self.pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.0 3.0")
        self.pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0")
        self.pg.parseCmd(":comheight 0.814")
        self.pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa")

        plug(dyn.position, self.pg.position)
        plug(dyn.com, self.pg.com)
        self.pg.motorcontrol.value = robotDim * (0, )
        self.pg.zmppreviouscontroller.value = (0, 0, 0)

        self.pg.initState()

        # --- PG INIT FRAMES ---
        self.geom = Dynamic("geom")
        self.geom.setFiles(modelDir, modelName, specificitiesPath,
                           jointRankPath)
        self.geom.parse()
        self.geom.createOpPoint('rf', 'right-ankle')
        self.geom.createOpPoint('lf', 'left-ankle')
        plug(dyn.position, self.geom.position)
        self.geom.ffposition.value = 6 * (0, )
        self.geom.velocity.value = robotDim * (0, )
        self.geom.acceleration.value = robotDim * (0, )

        # --- Selector of Com Ref: when pg is stopped, pg.inprocess becomes 0
        self.comRef = Selector('comRef',
                               ['vector', 'ref', dyn.com, self.pg.comref])
        plug(self.pg.inprocess, self.comRef.selec)

        self.selecSupportFoot = Selector('selecSupportFoot', [
            'matrixHomo', 'pg_H_sf', self.pg.rightfootref, self.pg.leftfootref
        ], ['matrixHomo', 'wa_H_sf', self.geom.rf, self.geom.lf])
        plug(self.pg.SupportFoot, self.selecSupportFoot.selec)
        self.sf_H_wa = Inverse_of_matrixHomo('sf_H_wa')
        plug(self.selecSupportFoot.wa_H_sf, self.sf_H_wa.sin)
        self.pg_H_wa = Multiply_of_matrixHomo('pg_H_wa')
        plug(self.selecSupportFoot.pg_H_sf, self.pg_H_wa.sin1)
        plug(self.sf_H_wa.sout, self.pg_H_wa.sin2)

        # --- Compute the ZMP ref in the Waist reference frame.
        self.wa_H_pg = Inverse_of_matrixHomo('wa_H_pg')
        plug(self.pg_H_wa.sout, self.wa_H_pg.sin)
        self.wa_zmp = Multiply_matrixHomo_vector('wa_zmp')
        plug(self.wa_H_pg.sout, self.wa_zmp.sin1)
        plug(self.pg.zmpref, self.wa_zmp.sin2)

        # --- Build the converter object for the waist task
        self.waistReferenceVector = Stack_of_vector('waistReferenceVector')
        plug(self.pg.initwaistposref, self.waistReferenceVector.sin1)
        # plug(self.pg.initwaistattref,self.waistReferenceVector.sin2)
        plug(self.pg.comattitude, self.waistReferenceVector.sin2)
        self.waistReferenceVector.selec1(0, 3)
        self.waistReferenceVector.selec2(0, 3)

        self.waistReference = PoseRollPitchYawToMatrixHomo('waistReference')
        plug(self.waistReferenceVector.sout, self.waistReference.sin)
        folder = rospack.get_path('sot-talos-balance') + "/data/" + test_folder
    else:
        folder = test_folder
    if folder[-1] != '/':
        folder += '/'

# --- Trajectory generators

# --- General trigger
robot.triggerTrajGen = BooleanIdentity('triggerTrajGen')
robot.triggerTrajGen.sin.value = 0

# --- CoM
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
robot.comTrajGen.x.recompute(0)  # trigger computation of initial value
plug(robot.triggerTrajGen.sout, robot.comTrajGen.trigger)

# --- Left foot
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
robot.lfTrajGen.x.recompute(0)  # trigger computation of initial value

robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
plug(robot.triggerTrajGen.sout, robot.lfTrajGen.trigger)

# --- Right foot
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
robot.rfTrajGen.x.recompute(0)  # trigger computation of initial value

robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
Example #46
0
 def plugWaistTask(self, taskWaist):
     plug(self.waistReference.sout, taskWaist.featureDes.position)
     taskWaist.feature.selec.value = '111100'
Example #47
0
print("Number of dofs (6 for the freeflyer + num of joints):")
print robot.dimension

# FIXME: this must be set so that the graph can be evaluated.
robot.device.zmp.value = (0., 0., 0.)

# Create a solver.
from dynamic_graph.sot.dyninv import SolverKine


def toList(solver):
    return map(lambda x: x[1:-1], solver.dispStack().split('|')[1:])


SolverKine.toList = toList
solver = SolverKine('sot')
solver.setSize(robot.dimension)
# set a constant damping to 0.1
solver.damping.value = 0.1

robot.device.control.unplug()
plug(solver.control, robot.device.control)
# Close the control loop
plug(robot.device.state, robot.dynamic.position)

print("...done!")

# Make sure only robot and solver are visible from the outside.
__all__ = ["robot", "solver"]
robot.com_traj_gen = create_com_traj_gen(robot, dt)
robot.com_traj_gen.x.recompute(0)
# --- Base orientation (SE3 on the waist) trajectory
robot.waist_traj_gen = create_waist_traj_gen("tg_waist_ref", robot, dt)
robot.waist_traj_gen.x.recompute(0)

# --- Simple inverse dynamic controller
robot.inv_dyn = create_simple_inverse_dyn_controller(robot, conf.balance_ctrl, dt)
robot.inv_dyn.setControlOutputType("torque")

# --- Connect control manager
connect_ctrl_manager(robot)

# --- Error on the CoM task
robot.errorComTSID = Substract_of_vector('error_com')
plug(robot.inv_dyn.com_ref_pos, robot.errorComTSID.sin2)
plug(robot.dynamic.com, robot.errorComTSID.sin1)

# --- Error on the Posture task
robot.errorPoseTSID = Substract_of_vector('error_pose')
plug(robot.inv_dyn.posture_ref_pos, robot.errorPoseTSID.sin2)
plug(robot.encoders.sout, robot.errorPoseTSID.sin1)


# # # --- ROS PUBLISHER ----------------------------------------------------------

robot.publisher = create_rospublish(robot, 'robot_publisher')
create_topic(robot.publisher, robot.errorPoseTSID, 'sout', 'errorPoseTSID', robot=robot, data_type='vector')  
create_topic(robot.publisher, robot.errorComTSID, 'sout', 'errorComTSID', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.dynamic, 'com', 'dynCom', robot=robot, data_type='vector') 
create_topic(robot.publisher, robot.inv_dyn, 'q_des', 'q_des', robot=robot, data_type='vector')
Example #49
0
 def plugTask(self):
     self.task.add(self.feature.name)
     plug(self.task.error,self.gain.error)
     plug(self.gain.gain,self.task.controlGain)
Example #50
0
        folder = rospack.get_path('sot_talos_balance') + "/data/" + test_folder
    else:
        folder = test_folder
    if folder[-1] != '/':
        folder += '/'

# --- Trajectory generators

# --- General trigger
robot.triggerTrajGen = BooleanIdentity('triggerTrajGen')
robot.triggerTrajGen.sin.value = 0

# --- CoM
robot.comTrajGen = create_com_trajectory_generator(dt, robot)
robot.comTrajGen.x.recompute(0)  # trigger computation of initial value
plug(robot.triggerTrajGen.sout, robot.comTrajGen.trigger)

# --- Left foot
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
robot.lfTrajGen.x.recompute(0)  # trigger computation of initial value

robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)
plug(robot.triggerTrajGen.sout, robot.lfTrajGen.trigger)

# --- Right foot
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
robot.rfTrajGen.x.recompute(0)  # trigger computation of initial value

robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)
# modify file /opt/ros/indigo/lib/python2.7/dist-packages/rqt_py_console/spyder_console_widget.py (moduleCompletion -> module_completion)

# file to modify joint bounds: 
# nano ~/devel/hrp2-n14-system/src/direct-access/hrp2-io-boards-init.cpp

from dynamic_graph.sot.torque_control.main import *
from dynamic_graph.sot.torque_control.utils.sot_utils import smoothly_set_signal,  stop_sot
from dynamic_graph import plug
robot.timeStep=0.0015
robot = main_v3(robot, startSoT=True, go_half_sitting=False)
go_to_position(robot.traj_gen, 30*(0.0,), 5.0)

robot.base_estimator.set_imu_weight(0.0)
plug(robot.filters.estimator_kin.dx,         robot.base_estimator.joint_velocities);
#plug(robot.filters.estimator_kin.dx,         robot.current_ctrl.dq);
#plug(robot.filters.estimator_kin.dx,         robot.torque_ctrl.jointsVelocities);
robot.inv_dyn.kp_com.value = (30.0, 30.0, 50.0)

# create ros topics
create_topic(robot.ros, robot.device.robotState,                'robotState')
create_topic(robot.ros, robot.estimator_ft.jointsTorques,       'tau');
create_topic(robot.ros, robot.torque_ctrl.jointsTorquesDesired, 'tau_des');
create_topic(robot.ros, robot.inv_dyn.com,                      'com')
create_topic(robot.ros, robot.inv_dyn.com_vel,                  'com_vel')
create_topic(robot.ros, robot.inv_dyn.com_ref_pos,              'com_ref_pos')
create_topic(robot.ros, robot.inv_dyn.com_ref_vel,              'com_ref_vel')
create_topic(robot.ros, robot.inv_dyn.com_acc_des,              'com_acc_des')
create_topic(robot.ros, robot.base_estimator.lf_xyzquat,        'lf_est')
create_topic(robot.ros, robot.base_estimator.rf_xyzquat,        'rf_est')
create_topic(robot.ros, robot.current_ctrl.i_real,              'i_real');
create_topic(robot.ros, robot.ctrl_manager.u,                   'i_des')
robot.dynamic.LF.recompute(0)
robot.dynamic.RF.recompute(0)
robot.dynamic.WT.recompute(0)

# -------------------------- DESIRED TRAJECTORY --------------------------

# --- Trajectory generators

# --- CoM
robot.comTrajGen = create_com_trajectory_generator(dt, robot)

# --- Left foot
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
# robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)

# --- Right foot
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
# robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)

# --- Waist
robot.waistTrajGen = create_orientation_rpy_trajectory_generator(
    dt, robot, 'WT')
# robot.waistTrajGen.x.recompute(0) # trigger computation of initial value

robot.waistMix = Mix_of_vector("waistMix")
robot.waistMix.setSignalNumber(3)
robot.waistMix.addSelec(1, 0, 3)
dt = robot.timeStep

# -------------------------- DESIRED TRAJECTORY --------------------------

robot.comTrajGen = create_com_trajectory_generator(dt, robot)

# -------------------------- ESTIMATION --------------------------

# --- Parameter server
robot.param_server = create_parameter_server(param_server_conf, dt)

# --- filters
filters = Bunch()
filters.ft_RF_filter = create_butter_lp_filter_Wn_04_N_2("ft_RF_filter", dt, 6)
filters.ft_LF_filter = create_butter_lp_filter_Wn_04_N_2("ft_LF_filter", dt, 6)
plug(robot.device.forceRLEG, filters.ft_RF_filter.x)
plug(robot.device.forceLLEG, filters.ft_LF_filter.x)
robot.device_filters = filters

# --- Force calibration
robot.ftc = create_ft_calibrator(robot, ft_conf)

# --- ZMP estimation (disconnected)
zmp_estimator = SimpleZmpEstimator("zmpEst")
robot.dynamic.createOpPoint('sole_LF', 'left_sole_link')
robot.dynamic.createOpPoint('sole_RF', 'right_sole_link')
plug(robot.dynamic.sole_LF, zmp_estimator.poseLeft)
plug(robot.dynamic.sole_RF, zmp_estimator.poseRight)
plug(robot.ftc.left_foot_force_out, zmp_estimator.wrenchLeft)
plug(robot.ftc.right_foot_force_out, zmp_estimator.wrenchRight)
zmp_estimator.init()
omega = sqrt(g / h)

# -------------------------- ESTIMATION --------------------------

# --- REAL ESTIMATION - diagnostics only

# --- Base Estimation
robot.param_server = create_parameter_server(param_server_conf, dt)
robot.device_filters = create_device_filters(robot, dt)
robot.imu_filters = create_imu_filters(robot, dt)
robot.base_estimator = create_base_estimator(robot, dt, base_estimator_conf)
# robot.be_filters              = create_be_filters(robot, dt)

# --- Conversion
e2q = EulerToQuat('e2q')
plug(robot.base_estimator.q, e2q.euler)
robot.e2q = e2q

# --- CoM Estimation
cdc_estimator = DcmEstimator('cdc_estimator')
cdc_estimator.init(dt, robot_name)
plug(robot.e2q.quaternion, cdc_estimator.q)
plug(robot.base_estimator.v, cdc_estimator.v)
robot.cdc_estimator = cdc_estimator

# --- SOT ESTIMATION

# --- DCM Estimation
estimator = DummyDcmEstimator("dummy")
estimator.omega.value = omega
estimator.mass.value = mass
robot.dynamic.LF.recompute(0)
robot.dynamic.RF.recompute(0)
robot.dynamic.WT.recompute(0)

# -------------------------- DESIRED TRAJECTORY --------------------------

# --- Trajectory generators

# --- CoM
robot.comTrajGen = create_com_trajectory_generator(dt, robot)

# --- Left foot
robot.lfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'LF')
# robot.lfTrajGen.x.recompute(0) # trigger computation of initial value
robot.lfToMatrix = PoseRollPitchYawToMatrixHomo('lf2m')
plug(robot.lfTrajGen.x, robot.lfToMatrix.sin)

# --- Right foot
robot.rfTrajGen = create_pose_rpy_trajectory_generator(dt, robot, 'RF')
# robot.rfTrajGen.x.recompute(0) # trigger computation of initial value
robot.rfToMatrix = PoseRollPitchYawToMatrixHomo('rf2m')
plug(robot.rfTrajGen.x, robot.rfToMatrix.sin)

# --- Waist
robot.waistTrajGen = create_orientation_rpy_trajectory_generator(
    dt, robot, 'WT')
# robot.waistTrajGen.x.recompute(0) # trigger computation of initial value

robot.waistMix = Mix_of_vector("waistMix")
robot.waistMix.setSignalNumber(3)
robot.waistMix.addSelec(1, 0, 3)
Example #56
0
 def plugEverything(self):
     self.feature.setReference(self.featureDes.name)
     plug(self.dyn.signal(self.opPoint), self.feature.signal('position'))
     plug(self.dyn.signal('J' + self.opPoint), self.feature.signal('Jq'))
     plug(self.dyn.signal(self.opPointBase),
          self.feature.signal('positionRef'))
     plug(self.dyn.signal('J' + self.opPointBase),
          self.feature.signal('JqRef'))
     self.task.add(self.feature.name)
     plug(self.task.error, self.gain.error)
     plug(self.gain.gain, self.task.controlGain)
def connect_ctrl_manager(ent):    
    # connect to device    
    plug(ent.device.robotState,             ent.ctrl_manager.base6d_encoders);
    plug(ent.device.currents,               ent.ctrl_manager.currents);
    plug(ent.estimator_kin.dx,              ent.ctrl_manager.dq);
    plug(ent.estimator_ft.jointsTorques,    ent.ctrl_manager.tau);
    plug(ent.ctrl_manager.pwmDes,           ent.torque_ctrl.pwm);
    ent.ctrl_manager.addCtrlMode("pos");
    ent.ctrl_manager.addCtrlMode("torque");    
    plug(ent.torque_ctrl.controlCurrent,        ent.ctrl_manager.ctrl_torque);
    plug(ent.pos_ctrl.pwmDes,                   ent.ctrl_manager.ctrl_pos);
    plug(ent.ctrl_manager.joints_ctrl_mode_torque,  ent.inv_dyn.active_joints);
    ent.ctrl_manager.setCtrlMode("all", "pos");
    plug(ent.ctrl_manager.pwmDesSafe,               ent.device.control);
    return;
Example #58
0
planeversor = FeatureAngleBtwPlaneAndVersor('planeversor')
pointpoint = FeaturePointToPoint('pointpoint')
planeversor.displaySignals()


# Define a task
taskExpg = Task('taskExpg')

#define the gain
taskExpg.controlGain.value = 1

# operational point used
opPoint1 = 'left-wrist' 
opPoint2 = 'right-wrist'
# Get the positions/Jacobians of the operational points for the dynamic entity
plug(robot.dynamic.signal(opPoint1),planeversor.signal('w_T_o1'))
plug(robot.dynamic.signal('J'+opPoint1),planeversor.signal('w_J_o1'))
plug(robot.dynamic.signal(opPoint2),planeversor.signal('w_T_o2'))
plug(robot.dynamic.signal('J'+opPoint2),planeversor.signal('w_J_o2'))

plug(robot.dynamic.signal(opPoint1),pointpoint.signal('w_T_o1'))
plug(robot.dynamic.signal('J'+opPoint1),pointpoint.signal('w_J_o1'))
plug(robot.dynamic.signal(opPoint2),pointpoint.signal('w_T_o2'))
plug(robot.dynamic.signal('J'+opPoint2),pointpoint.signal('w_J_o2'))
# Associate the feature to the task:
taskExpg.add(pointpoint.name)
taskExpg.add(planeversor.name)

#desired value
planeversor.reference.value = numpy.pi/2
pointpoint.reference.value = 0.0001
from dynamic_graph.sot.core.meta_tasks_kine import *
from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread,loopShortcuts
from dynamic_graph.sot.core.utils.viewer_helper import addRobotViewer,VisualPinger,updateComDisplay

from numpy import *
def totuple( a ):
    al=a.tolist()
    res=[]
    for i in range(a.shape[0]):
        res.append( tuple(al[i]) )
    return tuple(res)

# Create the robot.
from dynamic_graph.sot.romeo.romeo import *
robot = Robot('romeo', device=RobotSimu('romeo'))
plug(robot.device.state, robot.dynamic.position)

# Binds with ros, export joint_state.
from dynamic_graph.ros import *
ros = Ros(robot)

# Create a solver.
from dynamic_graph.sot.application.velocity.precomputed_tasks import Solver

solver = Solver(robot)

# Alternate visualization tool
addRobotViewer(robot.device,small=True,small_extra=24,verbose=False)


#-------------------------------------------------------------------------------
Example #60
0
 def plugZmp(self, robot):
     # Connect the ZMPref to OpenHRP in the waist reference frame.
     self.pg.parseCmd(':SetZMPFrame world')
     plug(self.wa_zmp.sout, robot.zmp)