def initializeTracer(self):
     if not self.tracer:
         self.tracer = TracerRealTime('trace')
         self.tracer.setBufferSize(self.tracerSize)
         self.tracer.open('/tmp/','dg_','.dat')
         # Recompute trace.triger at each iteration to enable tracing.
         self.device.after.addSignal('{0}.triger'.format(self.tracer.name))
예제 #2
0
    def initialize_tracer(self):
        """
        Initialize the tracer and by default dump the files in
         ~/dynamic_graph/[date_time]/
        """
        if not self.tracer:
            self.tracer = TracerRealTime('trace')
            self.tracer.setBufferSize(self.tracerSize)

        # Recompute trace.triger at each iteration to enable tracing.
        self.device.after.addSignal('{0}.triger'.format(self.tracer.name))
예제 #3
0
 def initializeTracer(self):
     if not self.tracer:
         self.tracer = TracerRealTime("trace")
         self.tracer.setBufferSize(self.tracerSize)
         self.tracer.open("/tmp/", "dg_", ".dat")
         # Recompute trace.triger at each iteration to enable tracing.
         self.device.after.addSignal("{0}.triger".format(self.tracer.name))
예제 #4
0
    def addTracerRealTime (self, robot):
        from dynamic_graph.tracer_real_time import TracerRealTime
        from agimus_sot.tools import filename_escape
        self._tracer = TracerRealTime (self.name + "_tracer")
        self._tracer.setBufferSize (10 * 1048576) # 10 Mo
        self._tracer.open ("/tmp", filename_escape(self.name), ".txt")

        self._tracer.add (self.omega2theta.name + ".sin1",        "_theta_current")
        self._tracer.add (self.omega2theta.name + ".sin2",        "_omega_desired")
        self._tracer.add (self.omega2theta.name + ".sout",        "_theta_desired")
        self._tracer.add (self.torque_controller.referenceName,   "_reference_torque")
        self._tracer.add (self.torque_controller.measurementName, "_measured_torque")
        if hasattr(self, "_current_selec"):
            self._tracer.add (self._current_selec.name + ".sout", "_measured_current")

        robot.device.after.addSignal(self._tracer.name + ".triger")
        return self._tracer
예제 #5
0
def dump_sot_sigs(robot, list_of_sigs, duration):
    '''dumps several sot signals in /tmp
    ex: dump_sot_sig(robot,[entity,signals],1.)'''
    tracer = TracerRealTime('tmp_tracer')
    tracer.setBufferSize(80 * (2**20))
    tracer.open('/tmp', 'dg_', '.dat')
    robot.device.after.addSignal('{0}.triger'.format(tracer.name))
    for sigs in list_of_sigs:
        entity = sigs[0]
        for sig in sigs[1:]:
            full_sig_name = entity.name + '.' + sig
            addTrace(tracer, entity, sig)
            robot.device.after.addSignal(full_sig_name)
    tracer.start()
    sleep(duration)
    dump_tracer(tracer)
    tracer.clear()
def create_tracer(device, traj_gen=None, estimator_ft=None, estimator_kin=None,
                  inv_dyn=None, torque_ctrl=None):
    tracer = TracerRealTime('motor_id_trace');
    tracer.setBufferSize(80*(2**20));
    tracer.open('/tmp/','dg_','.dat');
    device.after.addSignal('{0}.triger'.format(tracer.name));

    addSignalsToTracer(tracer, device);
        
    with open('/tmp/dg_info.dat', 'a') as f:
        if(estimator_ft!=None):
            f.write('Estimator F/T sensors delay: {0}\n'.format(robot.estimator_ft.getDelayFTsens()));
            f.write('Estimator use reference velocities: {0}\n'.format(robot.estimator_ft.getUseRefJointVel()));
            f.write('Estimator use reference accelerations: {0}\n'.format(robot.estimator_ft.getUseRefJointAcc()));
            f.write('Estimator accelerometer delay: {0}\n'.format(robot.estimator_ft.getDelayAcc()));
            f.write('Estimator gyroscope delay: {0}\n'.format(robot.estimator_ft.getDelayGyro()));
            f.write('Estimator use raw encoders: {0}\n'.format(robot.estimator_ft.getUseRawEncoders()));
            f.write('Estimator use f/t sensors: {0}\n'.format(robot.estimator_ft.getUseFTsensors()));
            f.write('Estimator f/t sensor offsets: {0}\n'.format(robot.estimator_ft.getFTsensorOffsets()));
        if(estimator_kin!=None):
            f.write('Estimator encoder delay: {0}\n'.format(robot.estimator_kin.getDelay()));
        if(inv_dyn!=None):
            f.write('Inv dyn Ks: {0}\n'.format(inv_dyn.Kp.value));
            f.write('Inv dyn Kd: {0}\n'.format(inv_dyn.Kd.value));
            f.write('Inv dyn Kf: {0}\n'.format(inv_dyn.Kf.value));
            f.write('Inv dyn Ki: {0}\n'.format(inv_dyn.Ki.value));
        if(torque_ctrl!=None):
            f.write('Torque ctrl KpTorque: {0}\n'.format (robot.torque_ctrl.KpTorque.value ));
            f.write('Torque ctrl KpCurrent: {0}\n'.format(robot.torque_ctrl.KpCurrent.value));
            f.write('Torque ctrl K_tau: {0}\n'.format(robot.torque_ctrl.k_tau.value));
            f.write('Torque ctrl K_v: {0}\n'.format(robot.torque_ctrl.k_v.value));
    f.close();
    return tracer;
예제 #7
0
파일: factory.py 프로젝트: ksyy/agimus-sot
    def generate(self):
        if self.parameters["addTimerToSotControl"]:
            # init tracer
            from dynamic_graph.tracer_real_time import TracerRealTime
            SoTtracer = TracerRealTime("tracer_of_timers")
            self.tracers[SoTtracer.name] = SoTtracer
            SoTtracer.setBufferSize(10 * 1048576)  # 10 Mo
            SoTtracer.open("/tmp", "sot-control-trace", ".txt")
            self.sotrobot.device.after.addSignal("tracer_of_timers.triger")
            self.supervisor.SoTtracer = SoTtracer
        super(Factory, self).generate()

        self.supervisor.sots = {}
        self.supervisor.grasps = {(gh, w): t
                                  for gh, ts in self.tasks._grasp.items()
                                  for w, t in ts.items()}
        self.supervisor.hpTasks = self.hpTasks
        self.supervisor.lpTasks = self.lpTasks
        self.supervisor.postActions = {}
        self.supervisor.preActions = {}
        self.supervisor.tracers = self.tracers
        self.supervisor.controllers = self.controllers

        from dynamic_graph import plug
        self.supervisor.sots_indexes = dict()
        for tn, sot in self.sots.iteritems():
            # Pre action
            if self.preActions.has_key(tn):
                self.supervisor.addPreAction(tn, self.preActions[tn])
            # Action
            self.supervisor.addSolver(tn, sot)
            # Post action
            if self.postActions.has_key(tn):
                self.supervisor.addPostActions(tn, self.postActions[tn])
예제 #8
0
def create_tracer(device,
                  traj_gen=None,
                  estimator_kin=None,
                  inv_dyn=None,
                  torque_ctrl=None):
    tracer = TracerRealTime('motor_id_trace')
    tracer.setBufferSize(80 * (2**20))
    tracer.open('/tmp/', 'dg_', '.dat')
    device.after.addSignal('{0}.triger'.format(tracer.name))

    addSignalsToTracer(tracer, device)

    with open('/tmp/dg_info.dat', 'a') as f:
        if (estimator_kin != None):
            f.write('Estimator encoder delay: {0}\n'.format(
                robot.filters.estimator_kin.getDelay()))
        if (inv_dyn != None):
            f.write('Inv dyn Ks: {0}\n'.format(inv_dyn.Kp.value))
            f.write('Inv dyn Kd: {0}\n'.format(inv_dyn.Kd.value))
            f.write('Inv dyn Kf: {0}\n'.format(inv_dyn.Kf.value))
            f.write('Inv dyn Ki: {0}\n'.format(inv_dyn.Ki.value))
        if (torque_ctrl != None):
            f.write('Torque ctrl KpTorque: {0}\n'.format(
                robot.torque_ctrl.KpTorque.value))
    f.close()
    return tracer
예제 #9
0
파일: factory.py 프로젝트: nim65s/sot-hpp
    def generate(self):
        if self.addTimerToSotControl:
            # init tracer
            from dynamic_graph.tracer_real_time import TracerRealTime
            self.SoTtracer = TracerRealTime("tracer_of_timers")
            self.timerIds = []
            self.SoTtracer.setBufferSize(10 * 1048576)  # 10 Mo
            self.SoTtracer.open("/tmp", "sot-control-trace", ".txt")
            self.sotrobot.device.after.addSignal("tracer_of_timers.triger")
            self.supervisor.SoTtracer = self.SoTtracer
            self.supervisor.SoTtimerIds = self.timerIds
        super(Factory, self).generate()

        self.supervisor.sots = self.sots
        self.supervisor.grasps = {(gh, w): t
                                  for gh, ts in self.tasks._grasp.items()
                                  for w, t in ts.items()}
        self.supervisor.hpTasks = self.hpTasks
        self.supervisor.lpTasks = self.lpTasks
        self.supervisor.postActions = self.postActions
        self.supervisor.preActions = self.preActions
        self.supervisor.SoTtimers = self.timers
def create_tracer(robot, entity, tracer_name, outputs=None):
    tracer = TracerRealTime(tracer_name)
    tracer.setBufferSize(80 * (2**20))
    tracer.open('/tmp', 'dg_', '.dat')
    robot.device.after.addSignal('{0}.triger'.format(tracer.name))
    if outputs is not None:
        addSignalsToTracer(tracer, entity, outputs)
    return tracer
예제 #11
0
 def addTracer(name, prefix, dir="/tmp", suffix=".txt", size=10 * 1048576):
     # default size: 10Mo
     tracer = TracerRealTime (name)
     self.tracers[tracer.name] = tracer
     tracer.setBufferSize (size)
     tracer.open (dir, prefix, suffix)
     self.sotrobot.device.after.addSignal(name + ".triger")
     return tracer
예제 #12
0
def create_tracer(robot, dir_name):
    if not os.path.exists(dir_name):
        os.makedirs(dir_name)
    tracer = TracerRealTime('motor_id_trace')
    tracer.setBufferSize(80 * (2**20))
    tracer.open(dir_name, 'dg_', '.dat')
    robot.device.after.addSignal('{0}.triger'.format(tracer.name))
    addSignalsToTracer(tracer, robot)
    return tracer
    def setupTrace(self):
	self.trace = TracerRealTime('trace')
	self.trace.setBufferSize(2**20)
	self.trace.open('/tmp/','legs_follower_','.dat')
	
	self.trace.add('legs-follower.com', 'com')
	self.trace.add('legs-follower.zmp', 'zmp')
	self.trace.add('legs-follower.ldof', 'ldof')
	self.trace.add('legs-follower.waist', 'waist')
	self.trace.add(self.robot.device.name + '.state', 'state')
	self.trace.add(self.legsTask.name + '.error', 'errorLegs')
        self.trace.add(self.robot.comTask.name + '.error', 'errorCom')

        #self.trace.add('legs-follower.outputStart','start')
        #self.trace.add('legs-follower.outputYaw','yaw')
        self.trace.add('corba.planner_steps','steps')
        self.trace.add('corba.planner_outputGoal','goal')
        self.trace.add('corba.waist','waistMocap')
	self.trace.add('corba.left-foot','footMocap')
        self.trace.add('corba.table','tableMocap')
        self.trace.add('corba.bar','barMocap')
        self.trace.add('corba.chair','chairMocap')
	self.trace.add('corba.helmet','helmetMocap')
	self.trace.add('corba.planner_outputObs','obstacles')

        self.trace.add(self.robot.dynamic.name + '.left-ankle',
                       self.robot.dynamic.name + '-left-ankle')
        self.trace.add(self.robot.dynamic.name + '.right-ankle',
                       self.robot.dynamic.name + '-right-ankle')


	# Recompute trace.triger at each iteration to enable tracing.
	self.robot.device.after.addSignal('legs-follower.zmp')
	self.robot.device.after.addSignal('legs-follower.outputStart')
	self.robot.device.after.addSignal('legs-follower.outputYaw')
	self.robot.device.after.addSignal('corba.planner_steps')
	self.robot.device.after.addSignal('corba.planner_outputGoal')
	self.robot.device.after.addSignal('corba.waist')
	self.robot.device.after.addSignal('corba.left-foot')
	self.robot.device.after.addSignal('corba.table')
	self.robot.device.after.addSignal('corba.bar')
	self.robot.device.after.addSignal('corba.chair')
	self.robot.device.after.addSignal('corba.helmet')
	self.robot.device.after.addSignal('corba.planner_outputObs')
        self.robot.device.after.addSignal(self.robot.dynamic.name + '.left-ankle')
	self.robot.device.after.addSignal(self.robot.dynamic.name + '.right-ankle')
	self.robot.device.after.addSignal('trace.triger')
	return
예제 #14
0
class AdmittanceControl(object):
    """
    The torque controller is then use to maintain a desired force.
    It outputs a velocity command to be sent to entity Device.
    """
    def __init__(self, name, estimated_theta_closed, desired_torque, period,
                 nums, denoms):
        """
        - estimated_theta_closed: Use for the initial position control. It should correspond to a configuration in collision.
                                  The closer to contact configuration, the least the overshoot.
        - desired_torque: The torque to be applied on the object.
        - period: The SoT integration time.
        - nums, denoms: coefficient of the controller:
           \sum_i denoms[i] * d^i theta / dt^i = \sum_j nums[j] d^j torque / dt^j
        """
        self.name = name
        self.est_theta_closed = estimated_theta_closed
        self.desired_torque = desired_torque
        self.dt = period

        self._makeTorqueControl(nums, denoms)

        self._makeIntegrationOfVelocity()

    ### Feed-forward - contact phase
    def _makeTorqueControl(self, nums, denoms):
        from agimus_sot.control.controllers import Controller
        self.torque_controller = Controller(
            self.name + "_torque", nums, denoms, self.dt,
            [0. for _ in self.est_theta_closed])
        self.torque_controller.addFeedback()
        self.torque_controller.reference.value = self.desired_torque

    ### Internal method
    def _makeIntegrationOfVelocity(self):
        from dynamic_graph.sot.core import Add_of_vector
        self.omega2theta = Add_of_vector(self.name + "_omega2theta")
        self.omega2theta.setCoeff2(self.dt)
        # self.omega2theta.sin1 <- current position
        # self.omega2theta.sin2 <- desired velocity
        plug(self.torque_controller.output, self.omega2theta.sin2)

    ### Setup event to tell when object is grasped and simulate torque feedback.
    def setupFeedbackSimulation(self, mass, damping, spring, theta0):
        from agimus_sot.control.controllers import Controller
        from dynamic_graph.sot.core import Add_of_vector
        from agimus_sot.sot import DelayVector

        ## theta -> phi = theta - theta0
        self._sim_theta2phi = Add_of_vector(self.name + "_sim_theta2phi")
        self._sim_theta2phi.setCoeff1(1)
        self._sim_theta2phi.setCoeff2(-1)
        self._sim_theta2phi.sin2.value = theta0

        ## phi -> torque
        from dynamic_graph.sot.core.switch import SwitchVector
        from dynamic_graph.sot.core.operator import CompareVector
        # reverse = self.theta_open[0] > theta0[0]
        reverse = self.desired_torque[0] < 0
        self.sim_contact_condition = CompareVector(self.name +
                                                   "_sim_contact_condition")
        self.sim_contact_condition.setTrueIfAny(False)

        self._sim_torque = SwitchVector(self.name + "_sim_torque")
        self._sim_torque.setSignalNumber(2)

        plug(self.sim_contact_condition.sout, self._sim_torque.boolSelection)

        # If phi < 0 (i.e. sim_contact_condition.sout == 1) -> non contact phase
        # else          contact phase
        if reverse:
            plug(self._sim_theta2phi.sout, self.sim_contact_condition.sin2)
            self.sim_contact_condition.sin1.value = [
                0. for _ in self.est_theta_closed
            ]
        else:
            plug(self._sim_theta2phi.sout, self.sim_contact_condition.sin1)
            self.sim_contact_condition.sin2.value = [
                0. for _ in self.est_theta_closed
            ]
        # Non contact phase
        # torque = 0, done above
        # Contact phase
        self.phi2torque = Controller(self.name + "_sim_phi2torque", (
            spring,
            damping,
            mass,
        ), (1., ), self.dt, [0. for _ in self.est_theta_closed])
        #TODO if M != 0: phi2torque.pushNumCoef(((M,),))
        plug(self._sim_theta2phi.sout, self.phi2torque.reference)

        # Condition
        # if phi < 0 -> no contact -> torque = 0
        self._sim_torque.sin1.value = [0. for _ in self.est_theta_closed]
        # else       ->    contact -> phi2torque
        plug(self.phi2torque.output, self._sim_torque.sin0)

        plug(self._sim_torque.sout, self.currentTorqueIn)

    def readPositionsFromRobot(self, robot, jointNames):
        # TODO Compare current position to self.est_theta_closed and
        # so as not to overshoot this position.
        # Input formattting
        from dynamic_graph.sot.core.operator import Selec_of_vector
        self._joint_selec = Selec_of_vector(self.name + "_joint_selec")
        model = robot.dynamic.model
        for jn in jointNames:
            jid = model.getJointId(jn)
            assert jid < len(model.joints)
            jmodel = model.joints[jid]
            self._joint_selec.addSelec(jmodel.idx_v, jmodel.idx_v + jmodel.nv)
        plug(robot.dynamic.position, self._joint_selec.sin)
        self.setCurrentPositionIn(self._joint_selec.sout)

    ## - param torque_constants: Should take into account the motor torque constant and the gear ratio.
    ## - param first_order_filter: Add a first order filter to the current signal.
    def readCurrentsFromRobot(self,
                              robot,
                              jointNames,
                              torque_constants,
                              first_order_filter=False):
        # Input formattting
        from dynamic_graph.sot.core.operator import Selec_of_vector
        self._current_selec = Selec_of_vector(self.name + "_current_selec")
        model = robot.dynamic.model
        for jn in jointNames:
            jid = model.getJointId(jn)
            assert jid < len(model.joints)
            jmodel = model.joints[jid]
            # TODO there is no value for the 6 first DoF
            assert jmodel.idx_v >= 6
            self._current_selec.addSelec(jmodel.idx_v - 6,
                                         jmodel.idx_v - 6 + jmodel.nv)

        from dynamic_graph.sot.core.operator import Multiply_of_vector
        plug(robot.device.currents, self._current_selec.sin)
        self._multiply_by_torque_constants = Multiply_of_vector(
            self.name + "_multiply_by_torque_constants")
        self._multiply_by_torque_constants.sin0.value = torque_constants

        if first_order_filter:
            from agimus_sot.control.controllers import Controller
            self._first_order_current_filter = Controller(
                self.name + "_first_order_current_filter", (5., ), (
                    5.,
                    1.,
                ), self.dt, [0. for _ in self.desired_torque])
            plug(self._current_selec.sout,
                 self._first_order_current_filter.reference)
            plug(self._first_order_current_filter.output,
                 self._multiply_by_torque_constants.sin1)
        else:
            plug(self._current_selec.sout,
                 self._multiply_by_torque_constants.sin1)

        plug(self._multiply_by_torque_constants.sout, self.currentTorqueIn)

    def readTorquesFromRobot(self, robot, jointNames):
        # Input formattting
        from dynamic_graph.sot.core.operator import Selec_of_vector
        self._torque_selec = Selec_of_vector(self.name + "_torque_selec")
        model = robot.dynamic.model
        for jn in jointNames:
            jid = model.getJointId(jn)
            assert jid < len(model.joints)
            jmodel = model.joints[jid]
            # TODO there is no value for the 6 first DoF
            assert jmodel.idx_v >= 6
            self._torque_selec.addSelec(jmodel.idx_v - 6,
                                        jmodel.idx_v - 6 + jmodel.nv)
        plug(robot.device.ptorques, self._torque_selec.sin)

        plug(self._torque_selec.sout, self.currentTorqueIn)

    # TODO remove me
    def addOutputTo(self, robot, jointNames, mix_of_vector, sot=None):
        #TODO assert isinstance(mix_of_vector, Mix_of_vector)
        i = mix_of_vector.getSignalNumber()
        mix_of_vector.setSignalNumber(i + 1)
        plug(self.outputVelocity, mix_of_vector.signal("sin" + str(i)))
        model = robot.dynamic.model
        for jn in jointNames:
            jid = model.getJointId(jn)
            jmodel = model.joints[jid]
            mix_of_vector.addSelec(i, jmodel.idx_v, jmodel.nv)

    def addTracerRealTime(self, robot):
        from dynamic_graph.tracer_real_time import TracerRealTime
        from agimus_sot.tools import filename_escape
        self._tracer = TracerRealTime(self.name + "_tracer")
        self._tracer.setBufferSize(10 * 1048576)  # 10 Mo
        self._tracer.open("/tmp", filename_escape(self.name), ".txt")

        self._tracer.add(self.omega2theta.name + ".sin1", "_theta_current")
        self._tracer.add(self.omega2theta.name + ".sin2", "_omega_desired")
        self._tracer.add(self.omega2theta.name + ".sout", "_theta_desired")
        self._tracer.add(self.torque_controller.referenceName,
                         "_reference_torque")
        self._tracer.add(self.torque_controller.measurementName,
                         "_measured_torque")
        if hasattr(self, "_current_selec"):
            self._tracer.add(self._current_selec.name + ".sout",
                             "_measured_current")

        robot.device.after.addSignal(self._tracer.name + ".triger")
        return self._tracer

    @property
    def outputPosition(self):
        return self.omega2theta.sout

    @property
    def referenceTorqueIn(self):
        return self.torque_controller.reference

    def setCurrentPositionIn(self, sig):
        plug(sig, self.omega2theta.sin1)
        if hasattr(self, "_sim_theta2phi"):
            plug(sig, self._sim_theta2phi.sin1)

    @property
    def currentTorqueIn(self):
        return self.torque_controller.measurement

    @property
    def torqueConstants(self):
        return self._multiply_by_torque_constants.sin0
예제 #15
0
class Robot(object):
    """
    This class instantiates a robot
    """

    init_pos = (0.0)
    init_vel = (0.0)
    init_acc = (0.0)
    """
    Tracer used to log data.
    """
    tracer = None
    """
    How much data will be logged.
    """
    tracerSize = 2**22
    """
    Automatically recomputed signals through the use
    of device.after.
    This list is maintained in order to clean the
    signal list device.after before exiting.
    """
    autoRecomputedSignals = []
    """
    Robot timestep
    """
    timeStep = 0.005

    def __init__(self, name, device=None, tracer=None):
        self.name = name
        self.device = device
        # Initialize tracer if necessary.
        if tracer:
            self.tracer = tracer
        self.initialize_tracer()

        # We trace by default all signals of the device.
        self.device_signals_names = []
        for signal in self.device.signals():
            signal_name = signal.name.split('::')[-1]
            self.add_trace(self.device.name, signal_name)
            self.device_signals_names.append(signal_name)

        # Prepare potential ros import/export
        self.ros = Ros(self.device)
        # self.export_device_dg_to_ros()

    def __del__(self):
        if self.tracer:
            self.stop_tracer()

    def add_trace(self, entityName, signalName):
        if self.tracer:
            addTrace(self, self.tracer, entityName, signalName)

    def _tracer_log_dir(self):
        import os
        import os.path
        import time
        log_dir = os.path.join(os.path.expanduser("~"),
                               "dynamic_graph_manager",
                               time.strftime("%Y-%m-%d_%H-%M-%S"))

        if not os.path.exists(log_dir):
            os.makedirs(log_dir)
        return log_dir

    def initialize_tracer(self):
        """
        Initialize the tracer and by default dump the files in
         ~/dynamic_graph/[date_time]/
        """
        if not self.tracer:
            self.tracer = TracerRealTime('trace')
            self.tracer.setBufferSize(self.tracerSize)

        # Recompute trace.triger at each iteration to enable tracing.
        self.device.after.addSignal('{0}.triger'.format(self.tracer.name))

    def start_tracer(self):
        """
        Start the tracer if it has not already been stopped.
        """
        if self.tracer:
            self.tracer_log_dir = self._tracer_log_dir()
            self.tracer.open(self.tracer_log_dir, 'dg_', '.dat')
            self.tracer.start()

    def stop_tracer(self):
        """
        Stop and destroy tracer.
        """
        if self.tracer:
            self.tracer.dump()
            self.tracer.stop()
            self.tracer.close()
            print("Stored trace in:", self.tracer_log_dir)

            # NOTE: Not calling self.tracer.clear() here, as by default the
            # tracer should keep it's traced signals attached.

            # Null the tracer object, such that initialize_tracer() will reopen it.
            self.trace = None

            self.initialize_tracer()

    def add_to_ros(self,
                   entityName,
                   signalName,
                   topic_name=None,
                   topic_type=None):
        """
        arg: topic_type is a string among:
              ['double', 'matrix', 'vector', 'vector3', 'vector3Stamped',
              'matrixHomo', 'matrixHomoStamped', 'twist', 'twistStamped',
              'joint_states'].
             Each different strings correspond to a ros message. For rviz
             support please use joint_states which correspond to the joint
             states including the potential free flyer joint.
        """
        # Lookup the entity's signal by name
        signal = Entity.entities[entityName].signal(signalName)

        if topic_name is None:
            topic_name = "/dg__" + entityName + '__' + signalName
            new_signal_name = "dg__" + entityName + '__' + signalName
        if topic_type is None:
            topic_type = "vector"

        self.ros.rosPublish.add(topic_type, new_signal_name, topic_name)
        plug(signal, self.ros.rosPublish.signal(new_signal_name))

    def add_robot_state_to_ros(self, entity_name, signal_name, base_link_name,
                               joint_names, tf_prefix, joint_state_topic_name):
        # Lookup the entity's signal by name
        signal = Entity.entities[entity_name].signal(signal_name)

        new_signal_name = "dg__" + entity_name + '__' + signal_name

        joint_names_string = ""
        for s in joint_names:
            joint_names_string += s + " "

        self.ros.rosRobotStatePublisher.add(
            base_link_name,
            joint_names_string,
            tf_prefix,
            new_signal_name,
            joint_state_topic_name,
        )
        plug(signal, self.ros.rosRobotStatePublisher.signal(new_signal_name))

    def add_ros_and_trace(self,
                          entityName,
                          signalName,
                          topic_name=None,
                          topic_type=None):
        self.add_trace(entityName, signalName)
        self.add_to_ros(entityName,
                        signalName,
                        topic_name=topic_name,
                        topic_type=topic_type)

    def export_device_dg_to_ros(self):
        """
        Import in ROS the signal from the dynamic graph device.
        """
        for sig_name in self.device_signals_names:
            self.add_to_ros(self.device.name, sig_name)
예제 #16
0
파일: demo.py 프로젝트: bcoudrin/sot_ur
feature_wrist = FeaturePosition ('position_wrist', model.wrist, model.Jwrist, I4)
task_wrist = Task ('wrist_task')
task_wrist.controlGain.value = 1.
task_wrist.add (feature_wrist.name)
# Create operational point for the end effector
model.createOpPoint ("ee", "ee_fixed_joint")

solver = SOT ('solver')
solver.setSize (dimension)
solver.push (task_wrist.name)

plug (solver.control, device.control)
device.increment (0.01)

#Create tracer
tracer = TracerRealTime ('trace')
tracer.setBufferSize(2**20)
tracer.open('/tmp/','dg_','.dat')
# Make sure signals are recomputed even if not used in the control graph
device.after.addSignal('{0}.triger'.format(tracer.name))
addTrace (device, tracer, device.name, "state")
addTrace (device, tracer, feature_wrist._feature.name, "position")
addTrace (device, tracer, feature_wrist._reference.name, "position")

#writeGraph('/tmp/graph')

dt = .01
tracer.start ()
for i in range (1000):
	device.increment (dt)
tracer.stop ()
def init_sot_talos_balance(robot, test_folder):
    cm_conf.CTRL_MAX = 1000.0  # temporary hack
    dt = robot.device.getTimeStep()
    robot.timeStep = dt

    # --- Pendulum parameters
    robot_name = 'robot'
    robot.dynamic.com.recompute(0)
    robotDim = robot.dynamic.getDimension()
    mass = robot.dynamic.data.mass[0]
    h = robot.dynamic.com.value[2]
    g = 9.81
    omega = sqrt(g / h)

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

    # --- Initial feet and waist
    robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle'])
    robot.dynamic.createOpPoint('RF',
                                robot.OperationalPointsMap['right-ankle'])
    robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist'])
    robot.dynamic.LF.recompute(0)
    robot.dynamic.RF.recompute(0)
    robot.dynamic.WT.recompute(0)

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

    rospack = RosPack()

    data_folder = rospack.get_path('sot-talos-balance') + "/data/"
    folder = data_folder + test_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
    robot.comTrajGen.playTrajectoryFile(folder + 'CoM.dat')
    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)
    robot.lfTrajGen.playTrajectoryFile(folder + 'LeftFoot.dat')
    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)
    robot.rfTrajGen.playTrajectoryFile(folder + 'RightFoot.dat')
    plug(robot.triggerTrajGen.sout, robot.rfTrajGen.trigger)

    # --- ZMP
    robot.zmpTrajGen = create_zmp_trajectory_generator(dt, robot)
    robot.zmpTrajGen.x.recompute(0)  # trigger computation of initial value
    # robot.zmpTrajGen.playTrajectoryFile(folder + 'ZMP.dat')
    plug(robot.triggerTrajGen.sout, robot.zmpTrajGen.trigger)

    # --- 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)
    robot.waistMix.addSelec(2, 3, 3)
    robot.waistMix.default.value = [0.0] * 6
    robot.waistMix.signal("sin1").value = [0.0] * 3
    plug(robot.waistTrajGen.x, robot.waistMix.signal("sin2"))

    robot.waistToMatrix = PoseRollPitchYawToMatrixHomo('w2m')
    plug(robot.waistMix.sout, robot.waistToMatrix.sin)
    robot.waistTrajGen.playTrajectoryFile(folder + 'WaistOrientation.dat')
    plug(robot.triggerTrajGen.sout, robot.waistTrajGen.trigger)

    # --- Interface with controller entities

    wp = DummyWalkingPatternGenerator('dummy_wp')
    wp.init()
    wp.omega.value = omega
    #wp.waist.value = robot.dynamic.WT.value          # wait receives a full homogeneous matrix, but only the rotational part is controlled
    #wp.footLeft.value = robot.dynamic.LF.value
    #wp.footRight.value = robot.dynamic.RF.value
    #wp.com.value  = robot.dynamic.com.value
    #wp.vcom.value = [0.]*3
    #wp.acom.value = [0.]*3
    plug(robot.waistToMatrix.sout, wp.waist)
    plug(robot.lfToMatrix.sout, wp.footLeft)
    plug(robot.rfToMatrix.sout, wp.footRight)
    plug(robot.comTrajGen.x, wp.com)
    plug(robot.comTrajGen.dx, wp.vcom)
    plug(robot.comTrajGen.ddx, wp.acom)
    #plug(robot.zmpTrajGen.x, wp.zmp)

    robot.wp = wp

    # --- Compute the values to use them in initialization
    robot.wp.comDes.recompute(0)
    robot.wp.dcmDes.recompute(0)
    robot.wp.zmpDes.recompute(0)

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

    # --- Base Estimation
    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.m2qLF = MatrixHomoToPoseQuaternion('m2qLF')
    plug(robot.dynamic.LF, robot.m2qLF.sin)
    plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat)
    robot.m2qRF = MatrixHomoToPoseQuaternion('m2qRF')
    plug(robot.dynamic.RF, robot.m2qRF.sin)
    plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat)

    # --- 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)
    robot.cdc_estimator = cdc_estimator

    # --- DCM Estimation
    estimator = DummyDcmEstimator("dummy")
    estimator.omega.value = omega
    estimator.mass.value = 1.0
    plug(robot.cdc_estimator.c, estimator.com)
    plug(robot.cdc_estimator.dc, estimator.momenta)
    estimator.init()
    robot.estimator = estimator

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

    # --- ZMP estimation
    zmp_estimator = SimpleZmpEstimator("zmpEst")
    robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link')
    robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link')
    plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
    plug(robot.rdynamic.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()
    robot.zmp_estimator = zmp_estimator

    # -------------------------- ADMITTANCE CONTROL --------------------------

    # --- DCM controller
    Kp_dcm = [8.0] * 3
    Ki_dcm = [0.0, 0.0, 0.0]  # zero (to be set later)
    gamma_dcm = 0.2

    dcm_controller = DcmController("dcmCtrl")

    dcm_controller.Kp.value = Kp_dcm
    dcm_controller.Ki.value = Ki_dcm
    dcm_controller.decayFactor.value = gamma_dcm
    dcm_controller.mass.value = mass
    dcm_controller.omega.value = omega

    plug(robot.cdc_estimator.c, dcm_controller.com)
    plug(robot.estimator.dcm, dcm_controller.dcm)

    plug(robot.wp.zmpDes, dcm_controller.zmpDes)
    plug(robot.wp.dcmDes, dcm_controller.dcmDes)

    dcm_controller.init(dt)

    robot.dcm_control = dcm_controller

    Ki_dcm = [1.0, 1.0, 1.0]  # this value is employed later

    # --- CoM admittance controller
    Kp_adm = [0.0, 0.0, 0.0]  # zero (to be set later)

    com_admittance_control = ComAdmittanceController("comAdmCtrl")
    com_admittance_control.Kp.value = Kp_adm
    plug(robot.zmp_estimator.zmp, com_admittance_control.zmp)
    com_admittance_control.zmpDes.value = robot.wp.zmpDes.value  # should be plugged to robot.dcm_control.zmpRef
    plug(robot.wp.acomDes, com_admittance_control.ddcomDes)

    com_admittance_control.init(dt)
    com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0])

    robot.com_admittance_control = com_admittance_control

    # --- Control Manager
    robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot')
    robot.cm.addCtrlMode('sot_input')
    robot.cm.setCtrlMode('all', 'sot_input')
    robot.cm.addEmergencyStopSIN('zmp')

    # -------------------------- SOT CONTROL --------------------------

    # --- Upper body
    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

    # robotDim = robot.dynamic.getDimension() # 38
    robot.taskUpperBody.feature.selectDof(18, True)
    robot.taskUpperBody.feature.selectDof(19, True)
    robot.taskUpperBody.feature.selectDof(20, True)
    robot.taskUpperBody.feature.selectDof(21, True)
    robot.taskUpperBody.feature.selectDof(22, True)
    robot.taskUpperBody.feature.selectDof(23, True)
    robot.taskUpperBody.feature.selectDof(24, True)
    robot.taskUpperBody.feature.selectDof(25, True)
    robot.taskUpperBody.feature.selectDof(26, True)
    robot.taskUpperBody.feature.selectDof(27, True)
    robot.taskUpperBody.feature.selectDof(28, True)
    robot.taskUpperBody.feature.selectDof(29, True)
    robot.taskUpperBody.feature.selectDof(30, True)
    robot.taskUpperBody.feature.selectDof(31, True)
    robot.taskUpperBody.feature.selectDof(32, True)
    robot.taskUpperBody.feature.selectDof(33, True)
    robot.taskUpperBody.feature.selectDof(34, True)
    robot.taskUpperBody.feature.selectDof(35, True)
    robot.taskUpperBody.feature.selectDof(36, True)
    robot.taskUpperBody.feature.selectDof(37, 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
    robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
                                     robot.OperationalPointsMap['left-ankle'])
    robot.contactLF.feature.frame('desired')
    robot.contactLF.gain.setConstant(300)
    plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position)  #.errorIN?
    locals()['contactLF'] = robot.contactLF

    robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
                                     robot.OperationalPointsMap['right-ankle'])
    robot.contactRF.feature.frame('desired')
    robot.contactRF.gain.setConstant(300)
    plug(robot.wp.footRightDes,
         robot.contactRF.featureDes.position)  #.errorIN?
    locals()['contactRF'] = robot.contactRF

    # --- COM height
    robot.taskComH = MetaTaskKineCom(robot.dynamic, name='comH')
    plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN)
    robot.taskComH.task.controlGain.value = 100.
    robot.taskComH.feature.selec.value = '100'

    # --- COM
    robot.taskCom = MetaTaskKineCom(robot.dynamic)
    plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
    plug(robot.com_admittance_control.dcomRef,
         robot.taskCom.featureDes.errordotIN)
    robot.taskCom.task.controlGain.value = 0
    robot.taskCom.task.setWithDerivative(True)
    robot.taskCom.feature.selec.value = '011'

    # --- Waist
    robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT',
                                     robot.OperationalPointsMap['waist'])
    robot.keepWaist.feature.frame('desired')
    robot.keepWaist.gain.setConstant(300)
    plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)
    robot.keepWaist.feature.selec.value = '111000'
    locals()['keepWaist'] = robot.keepWaist

    # --- SOT solver
    robot.sot = SOT('sot')
    robot.sot.setSize(robot.dynamic.getDimension())

    # --- Plug SOT control to device through control manager
    plug(robot.sot.control, robot.cm.ctrl_sot_input)
    plug(robot.cm.u_safe, robot.device.control)

    robot.sot.push(robot.taskUpperBody.name)
    robot.sot.push(robot.contactRF.task.name)
    robot.sot.push(robot.contactLF.task.name)
    robot.sot.push(robot.taskComH.task.name)
    robot.sot.push(robot.taskCom.task.name)
    robot.sot.push(robot.keepWaist.task.name)

    # --- Fix robot.dynamic inputs
    plug(robot.device.velocity, robot.dynamic.velocity)
    robot.dvdt = Derivator_of_Vector("dv_dt")
    robot.dvdt.dt.value = dt
    plug(robot.device.velocity, robot.dvdt.sin)
    plug(robot.dvdt.sout, robot.dynamic.acceleration)

    # -------------------------- PLOTS --------------------------

    # --- ROS PUBLISHER
    robot.publisher = create_rospublish(robot, 'robot_publisher')

    create_topic(robot.publisher,
                 robot.device,
                 'state',
                 robot=robot,
                 data_type='vector')
    create_topic(robot.publisher,
                 robot.base_estimator,
                 'q',
                 robot=robot,
                 data_type='vector')
    #create_topic(robot.publisher, robot.stf, 'q', robot = robot, data_type='vector')

    create_topic(robot.publisher,
                 robot.comTrajGen,
                 'x',
                 robot=robot,
                 data_type='vector')  # generated CoM
    create_topic(robot.publisher,
                 robot.comTrajGen,
                 'dx',
                 robot=robot,
                 data_type='vector')  # generated CoM velocity
    create_topic(robot.publisher,
                 robot.comTrajGen,
                 'ddx',
                 robot=robot,
                 data_type='vector')  # generated CoM acceleration

    create_topic(robot.publisher,
                 robot.wp,
                 'comDes',
                 robot=robot,
                 data_type='vector')  # desired CoM

    create_topic(robot.publisher,
                 robot.cdc_estimator,
                 'c',
                 robot=robot,
                 data_type='vector')  # estimated CoM
    create_topic(robot.publisher,
                 robot.cdc_estimator,
                 'dc',
                 robot=robot,
                 data_type='vector')  # estimated CoM velocity

    create_topic(robot.publisher,
                 robot.com_admittance_control,
                 'comRef',
                 robot=robot,
                 data_type='vector')  # reference CoM
    create_topic(robot.publisher,
                 robot.dynamic,
                 'com',
                 robot=robot,
                 data_type='vector')  # resulting SOT CoM

    create_topic(robot.publisher,
                 robot.dcm_control,
                 'dcmDes',
                 robot=robot,
                 data_type='vector')  # desired DCM
    create_topic(robot.publisher,
                 robot.estimator,
                 'dcm',
                 robot=robot,
                 data_type='vector')  # estimated DCM

    create_topic(robot.publisher,
                 robot.zmpTrajGen,
                 'x',
                 robot=robot,
                 data_type='vector')  # generated ZMP
    create_topic(robot.publisher,
                 robot.wp,
                 'zmpDes',
                 robot=robot,
                 data_type='vector')  # desired ZMP
    create_topic(robot.publisher,
                 robot.dynamic,
                 'zmp',
                 robot=robot,
                 data_type='vector')  # SOT ZMP
    create_topic(robot.publisher,
                 robot.zmp_estimator,
                 'zmp',
                 robot=robot,
                 data_type='vector')  # estimated ZMP
    create_topic(robot.publisher,
                 robot.dcm_control,
                 'zmpRef',
                 robot=robot,
                 data_type='vector')  # reference ZMP

    create_topic(robot.publisher,
                 robot.waistTrajGen,
                 'x',
                 robot=robot,
                 data_type='vector')  # desired waist orientation

    create_topic(robot.publisher,
                 robot.lfTrajGen,
                 'x',
                 robot=robot,
                 data_type='vector')  # desired left foot pose
    create_topic(robot.publisher,
                 robot.rfTrajGen,
                 'x',
                 robot=robot,
                 data_type='vector')  # desired right foot pose

    create_topic(robot.publisher,
                 robot.ftc,
                 'left_foot_force_out',
                 robot=robot,
                 data_type='vector')  # calibrated left wrench
    create_topic(robot.publisher,
                 robot.ftc,
                 'right_foot_force_out',
                 robot=robot,
                 data_type='vector')  # calibrated right wrench

    create_topic(robot.publisher,
                 robot.dynamic,
                 'LF',
                 robot=robot,
                 data_type='matrixHomo')  # left foot
    create_topic(robot.publisher,
                 robot.dynamic,
                 'RF',
                 robot=robot,
                 data_type='matrixHomo')  # right foot

    # --- TRACER
    robot.tracer = TracerRealTime("com_tracer")
    robot.tracer.setBufferSize(80 * (2**20))
    robot.tracer.open('/tmp', 'dg_', '.dat')
    robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))

    addTrace(robot.tracer, robot.wp, 'comDes')  # desired CoM

    addTrace(robot.tracer, robot.cdc_estimator, 'c')  # estimated CoM
    addTrace(robot.tracer, robot.cdc_estimator, 'dc')  # estimated CoM velocity

    addTrace(robot.tracer, robot.com_admittance_control,
             'comRef')  # reference CoM
    addTrace(robot.tracer, robot.dynamic, 'com')  # resulting SOT CoM

    addTrace(robot.tracer, robot.dcm_control, 'dcmDes')  # desired DCM
    addTrace(robot.tracer, robot.estimator, 'dcm')  # estimated DCM

    addTrace(robot.tracer, robot.dcm_control, 'zmpDes')  # desired ZMP
    addTrace(robot.tracer, robot.dynamic, 'zmp')  # SOT ZMP
    addTrace(robot.tracer, robot.zmp_estimator, 'zmp')  # estimated ZMP
    addTrace(robot.tracer, robot.dcm_control, 'zmpRef')  # reference ZMP

    addTrace(robot.tracer, robot.ftc,
             'left_foot_force_out')  # calibrated left wrench
    addTrace(robot.tracer, robot.ftc,
             'right_foot_force_out')  # calibrated right wrench

    addTrace(robot.tracer, robot.dynamic, 'LF')  # left foot
    addTrace(robot.tracer, robot.dynamic, 'RF')  # right foot

    robot.tracer.start()
class AbstractHumanoidRobot (object):
    """
    This class instantiates all the entities required to get a consistent
    representation of a humanoid robot, mainly:
      - device : to integrate velocities into angular control,
      - dynamic: to compute forward geometry and kinematics,
      - zmpFromForces: to compute ZMP force foot force sensors,
      - stabilizer: to stabilize balanced motions

    Operational points are stored into 'OperationalPoints' list. Some of them
    are also accessible directly as attributes:
      - leftWrist,
      - rightWrist,
      - leftAnkle,
      - rightAnkle,
      - Gaze.

    Tasks are stored into 'tasks' dictionary.

    For portability, some signals are accessible as attributes:
      - zmpRef: input (vector),
      - comRef: input (vector).
      - com:    output (vector)
      - comSelec input (flag)
      - comdot: input (vector) reference velocity of the center of mass

    """

    OperationalPoints = []
    """
    Operational points are specific interesting points of the robot
    used to control it.

    When an operational point is defined, signals corresponding to the
    point position and jacobian are created.

    For instance if creating an operational point for the left-wrist,
    the associated signals will be called "left-wrist" and
    "Jleft-wrist" for respectively the position and the jacobian.
    """

    AdditionalFrames = []
    """
    Additional frames are frames which are defined w.r.t an operational point
    and provides an interesting transformation.

    It can be used, for instance, to store the sensor location.

    The contained elements must be triplets matching:
    - additional frame name,
    - transformation w.r.t to the operational point,
    - operational point file.
    """

    name = None
    """Entity name (internal use)"""

    halfSitting = None
    """
    The half-sitting position is the robot initial pose.
    This attribute *must* be defined in subclasses.
    """

    dynamic = None
    """
    The robot dynamic model.
    """
    device = None
    """
    The device that integrates the dynamic equation, namely
      - the real robot or
      - a simulator
    """
    dimension = None
    """The configuration size."""

    featureCom = None
    """
    This generic feature takes as input the robot center of mass
    and as desired value the featureComDes feature of this class.
    """
    featureComDes = None
    """
    The feature associated to the robot center of mass desired
    position.
    """
    comTask = None

    features = dict()
    """
    Features associated to each operational point. Keys are
    corresponding to operational points.
    """
    tasks = dict()
    """
    Features associated to each operational point. Keys are
    corresponding to operational points.
    """

    frames = dict()
    """
    Additional frames defined by using OpPointModifier.
    """

    #FIXME: the following options are /not/ independent.
    # zmp requires acceleration which requires velocity.
    """
    Enable velocity computation.
    """
    enableVelocityDerivator = False
    """
    Enable acceleration computation.
    """
    enableAccelerationDerivator = False
    """
    Enable ZMP computation
    """
    enableZmpComputation = False

    """
    Tracer used to log data.
    """
    tracer = None

    """
    How much data will be logged.
    """
    tracerSize = 2**20

    """
    Automatically recomputed signals through the use
    of device.after.
    This list is maintained in order to clean the
    signal list device.after before exiting.
    """
    autoRecomputedSignals = []

    """
    Which signals should be traced.
    """
    tracedSignals = {
        'dynamic': ["com", "zmp", "angularmomentum",
                  "position", "velocity", "acceleration"],
        'device': ['zmp', 'control', 'state']
        }

    """
    Robot timestep
    """
    timeStep = 0.005

    def help (self):
        print (AbstractHumanoidRobot.__doc__)

    def loadModelFromKxml(self, name, filename):
        """
        Load a model from a kxml file and return the parsed model.
        This uses the Python parser class implement in
        dynamic_graph.sot.dynamics.parser.

        kxml is an extensible file format used by KineoWorks to store
        both the robot mesh and its kinematic chain.

        The parser also imports inertia matrices which is a
        non-standard property.
        """
        model = Parser(name, filename).parse()
        self.setProperties(model)
        return model

    def loadModelFromJrlDynamics(self, name, modelDir, modelName,
                                 specificitiesPath, jointRankPath,
                                 dynamicType):
        """
        Load a model using the jrl-dynamics parser. This parser looks
        for VRML files in which kinematics and dynamics information
        have been added by extending the VRML format.

        It is mainly used by OpenHRP.

        Additional information are located in two different XML files.
        """
        model = dynamicType(name)
        self.setProperties(model)
        model.setFiles(modelDir, modelName,
                       specificitiesPath, jointRankPath)
        model.parse()
        return model

    def setProperties(self, model):
        model.setProperty('TimeStep', str(self.timeStep))

        model.setProperty('ComputeAcceleration', 'false')
        model.setProperty('ComputeAccelerationCoM', 'false')
        model.setProperty('ComputeBackwardDynamics', 'false')
        model.setProperty('ComputeCoM', 'false')
        model.setProperty('ComputeMomentum', 'false')
        model.setProperty('ComputeSkewCom', 'false')
        model.setProperty('ComputeVelocity', 'false')
        model.setProperty('ComputeZMP', 'false')

        model.setProperty('ComputeAccelerationCoM', 'true')
        model.setProperty('ComputeCoM', 'true')
        model.setProperty('ComputeVelocity', 'true')
        model.setProperty('ComputeZMP', 'true')

        if self.enableZmpComputation:
            model.setProperty('ComputeBackwardDynamics', 'true')
            model.setProperty('ComputeAcceleration', 'true')
            model.setProperty('ComputeMomentum', 'true')


    def initializeOpPoints(self, model):
        for op in self.OperationalPoints:
            model.createOpPoint(op, op)

    def createCenterOfMassFeatureAndTask(self,
                                         featureName, featureDesName,
                                         taskName,
                                         selec = '011',
                                         gain = 1.):
        self.dynamic.com.recompute(0)
        self.dynamic.Jcom.recompute(0)

        featureCom = FeatureGeneric(featureName)
        plug(self.dynamic.com, featureCom.errorIN)
        plug(self.dynamic.Jcom, featureCom.jacobianIN)
        featureCom.selec.value = selec
        featureComDes = FeatureGeneric(featureDesName)
        featureComDes.errorIN.value = self.dynamic.com.value
        featureCom.setReference(featureComDes.name)
        comTask = Task(taskName)
        comTask.add(featureName)
        comTask.controlGain.value = gain
        return (featureCom, featureComDes, comTask)

    def createOperationalPointFeatureAndTask(self,
                                             operationalPointName,
                                             featureName,
                                             taskName,
                                             gain = .2):
        jacobianName = 'J{0}'.format(operationalPointName)
        self.dynamic.signal(operationalPointName).recompute(0)
        self.dynamic.signal(jacobianName).recompute(0)
        feature = \
            FeaturePosition(featureName,
                            self.dynamic.signal(operationalPointName),
                            self.dynamic.signal(jacobianName),
                            self.dynamic.signal(operationalPointName).value)
        task = Task(taskName)
        task.add(featureName)
        task.controlGain.value = gain
        return (feature, task)

    def createBalanceTask (self, taskName, gain = 1.):
        task = Task (taskName)
        task.add (self.featureCom.name)
        task.add (self.leftAnkle.name)
        task.add (self.rightAnkle.name)
        task.controlGain.value = gain
        return task

    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 initializeSignals (self):
        """
        For portability, make some signals accessible as attributes.
        """
        self.comRef = self.featureComDes.errorIN
        self.zmpRef = self.device.zmp
        self.com = self.dynamic.com
        self.comSelec = self.featureCom.selec
        self.comdot = self.featureComDes.errordotIN

    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)
        self.dynamic.position.value = self.halfSitting

        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.,)

        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.,)
        """
        self.initializeOpPoints(self.dynamic)
        
        # --- center of mass ------------
        (self.featureCom, self.featureComDes, self.comTask) = \
            self.createCenterOfMassFeatureAndTask(
            '{0}_feature_com'.format(self.name),
            '{0}_feature_ref_com'.format(self.name),
            '{0}_task_com'.format(self.name))

        # --- operational points tasks -----
        self.features = dict()
        self.tasks = dict()
        for op in self.OperationalPoints:
            (self.features[op], self.tasks[op]) = \
                self.createOperationalPointFeatureAndTask(
                op, '{0}_feature_{1}'.format(self.name, op),
                '{0}_task_{1}'.format(self.name, op))
            # define a member for each operational point
            w = op.split('-')
            memberName = w[0]
            for i in w[1:]:
                memberName += i.capitalize()
            setattr(self, memberName, self.features[op])
        self.tasks ['com'] = self.comTask

        # --- balance task --- #
        self.tasks ['balance'] =\
            self.createBalanceTask ('{0}_task_balance'.format (self.name))

        # --- additional frames ---
        self.frames = dict()
        frameName = 'rightHand'
        hp = self.dynamic.getHandParameter (True)
        transformation = list (map (list, I4))
        for i in range (3): transformation [i][3] = hp [i][3]
        transformation = tuple (map (tuple, transformation))
        self.frames [frameName] = self.createFrame (
            "{0}_{1}".format (self.name, frameName),
            transformation, "right-wrist")
        frameName = 'leftHand'
        hp = self.dynamic.getHandParameter (False)
        transformation = list (map (list, I4))
        for i in range (3): transformation [i][3] = hp [i][3]
        transformation = tuple (map (tuple, transformation))
        self.frames [frameName] = self.createFrame (
            "{0}_{1}".format (self.name, frameName),
            self.dynamic.getHandParameter (False), "left-wrist")

        for (frameName, transformation, signalName) in self.AdditionalFrames:
            self.frames[frameName] = self.createFrame(
                "{0}_{1}".format(self.name, frameName),
                transformation,
                signalName)
        self.initializeSignals ()
    """
    def addTrace(self, entityName, signalName):
        if self.tracer:
            self.autoRecomputedSignals.append(
                '{0}.{1}'.format(entityName, signalName))
            addTrace(self, self.tracer, entityName, signalName)

    def initializeTracer(self):
        if not self.tracer:
            self.tracer = TracerRealTime('trace')
            self.tracer.setBufferSize(self.tracerSize)
            self.tracer.open('/tmp/','dg_','.dat')
            # Recompute trace.triger at each iteration to enable tracing.
            self.device.after.addSignal('{0}.triger'.format(self.tracer.name))

    def traceDefaultSignals (self):
        # Geometry / operational points
        for s in self.OperationalPoints + self.tracedSignals['dynamic']:
            self.addTrace(self.dynamic.name, s)

        # Geometry / frames
        for (frameName, _, _) in self.AdditionalFrames:
            for s in ['position', 'jacobian']:
                self.addTrace(self.frames[frameName].name, s)

        # Robot features
        for s in self.OperationalPoints:
            self.addTrace(self.features[s]._reference.name, 'position')
            self.addTrace(self.tasks[s].name, 'error')

        # Com
        self.addTrace(self.featureComDes.name, 'errorIN')
        self.addTrace(self.comTask.name, 'error')

        # Device
        for s in self.tracedSignals['device']:
            self.addTrace(self.device.name, s)
        if type(self.device) != RobotSimu:
            self.addTrace(self.device.name, 'robotState')

        # Misc
        if self.enableVelocityDerivator:
            self.addTrace(self.velocityDerivator.name, 'sout')
        if self.enableAccelerationDerivator:
            self.addTrace(self.accelerationDerivator.name, 'sout')


    def __init__(self, name, tracer = None):
        self.name = name

        # Initialize tracer if necessary.
        if tracer:
            self.tracer = tracer

    def __del__(self):
        if self.tracer:
            self.stopTracer()

    def startTracer(self):
        """
        Start the tracer if it does not already been stopped.
        """
        if self.tracer:
            self.tracer.start()

    def stopTracer(self):
        """
        Stop and destroy tracer.
        """
        if self.tracer:
            self.tracer.dump()
            self.tracer.stop()
            self.tracer.close()
            self.tracer.clear()
            for s in self.autoRecomputedSignals:
                self.device.after.rmSignal(s)
            self.tracer = None

    def reset(self, posture = None):
        """
        Restart the control from another position.

        This method has not been extensively tested and
        should be used carefully.

        In particular, tasks should be removed from the
        solver before attempting a reset.
        """
        if not posture:
            posture = self.halfSitting
        self.device.set(posture)

        #self.dynamic.com.recompute(self.device.state.time+1)
        #self.dynamic.Jcom.recompute(self.device.state.time+1)
        #self.featureComDes.errorIN.value = self.dynamic.com.value

        for op in self.OperationalPoints:
            self.dynamic.signal(op).recompute(self.device.state.time+1)
            self.dynamic.signal('J'+op).recompute(self.device.state.time+1)
            self.features[op].reference.value = self.dynamic.signal(op).value
             robot.forceCalibrator,
             'rightWristForceOut',
             robot=robot,
             data_type='vector')  # calibrated right wrench

# # --- ROS SUBSCRIBER
robot.subscriber = RosSubscribe("end_effector_subscriber")
robot.subscriber.add("vector", "w_force", "/sot/controller/w_force")
robot.subscriber.add("vector", "force", "/sot/controller/force")
robot.subscriber.add("vector", "dq", "/sot/controller/dq")
robot.subscriber.add("vector", "w_dq", "/sot/controller/w_dq")
robot.subscriber.add("vector", "w_forceDes", "/sot/controller/w_forceDes")
robot.subscriber.add("vector", "leftWristForceOut",
                     "/sot/forceCalibrator/leftWristForceOut")
robot.subscriber.add("vector", "rightWristForceOut",
                     "/sot/forceCalibrator/rightWristForceOut")

# --- TRACER  ------------------------------------------------------------------

robot.tracer = TracerRealTime("force_tracer")
robot.tracer.setBufferSize(80 * (2**20))
robot.tracer.open('/tmp', 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))

addTrace(robot.tracer, robot.controller, 'force')
addTrace(robot.tracer, robot.controller, 'w_force')
addTrace(robot.tracer, robot.controller, 'w_dq')
addTrace(robot.tracer, robot.controller, 'dq')

robot.tracer.start()
예제 #20
0
class AbstractRobot(ABC):
    """
    This class instantiates all the entities required to get a consistent
    representation of a robot, mainly:
      - device : to integrate velocities into angular control,
      - dynamic: to compute forward geometry and kinematics,
      - zmpFromForces: to compute ZMP force foot force sensors,
      - stabilizer: to stabilize balanced motions

    Operational points are stored into 'OperationalPoints' list. Some of them
    are also accessible directly as attributes:
      - leftWrist,
      - rightWrist,
      - leftAnkle,
      - rightAnkle,
      - Gaze.

    Operational points are mapped to the actual joints in the robot model
    via 'OperationalPointsMap' dictionary.
    This attribute *must* be defined in the subclasses

    Other attributes require to be defined:
        - halfSitting: half-sitting position is the robot initial pose.
            This attribute *must* be defined in subclasses.

        - dynamic: The robot dynamic model.

        - device: The device that integrates the dynamic equation, namely
            the real robot or
            a simulator

        - dimension: The configuration size.
    """
    def _initialize(self):
        self.OperationalPoints = []
        """
        Operational points are specific interesting points of the robot
        used to control it.

        When an operational point is defined, signals corresponding to the
        point position and jacobian are created.

        For instance if creating an operational point for the left-wrist,
        the associated signals will be called "left-wrist" and
        "Jleft-wrist" for respectively the position and the jacobian.
        """

        self.AdditionalFrames = []
        """
        Additional frames are frames which are defined w.r.t an operational point
        and provides an interesting transformation.

        It can be used, for instance, to store the sensor location.

        The contained elements must be triplets matching:
        - additional frame name,
        - transformation w.r.t to the operational point,
        - operational point file.
        """

        self.frames = dict()
        """
        Additional frames defined by using OpPointModifier.
        """

        # FIXME: the following options are /not/ independent.
        # zmp requires acceleration which requires velocity.
        """
        Enable velocity computation.
        """
        self.enableVelocityDerivator = False
        """
        Enable acceleration computation.
        """
        self.enableAccelerationDerivator = False
        """
        Enable ZMP computation
        """
        self.enableZmpComputation = False
        """
        Tracer used to log data.
        """
        self.tracer = None
        """
        How much data will be logged.
        """
        self.tracerSize = 2**20
        """
        Automatically recomputed signals through the use
        of device.after.
        This list is maintained in order to clean the
        signal list device.after before exiting.
        """
        self.autoRecomputedSignals = []
        """
        Which signals should be traced.
        """
        self.tracedSignals = {
            'dynamic': [
                "com", "zmp", "angularmomentum", "position", "velocity",
                "acceleration"
            ],
            'device': ['zmp', 'control', 'state']
        }

    def help(self):
        print(AbstractHumanoidRobot.__doc__)

    def _removeMimicJoints(self, urdfFile=None, urdfString=None):
        """ Parse the URDF, extract the mimic joints and call removeJoints. """
        # get mimic joints
        import xml.etree.ElementTree as ET
        if urdfFile is not None:
            assert urdfString is None, "One and only one of input argument should be provided"
            root = ET.parse(urdfFile)
        else:
            assert urdfString is not None, "One and only one of input argument should be provided"
            root = ET.fromstring(urdfString)

        mimicJoints = list()
        for e in root.iter('joint'):
            if 'name' in e.attrib:
                name = e.attrib['name']
                for c in e:
                    if hasattr(c, 'tag') and c.tag == 'mimic':
                        mimicJoints.append(name)
        self.removeJoints(mimicJoints)

    def removeJoints(self, joints):
        """
        - param joints: a list of joint names to be removed from the self.pinocchioModel
        """
        jointIds = list()
        for j in joints:
            if self.pinocchioModel.existJointName(j):
                jointIds.append(self.pinocchioModel.getJointId(j))
        if len(jointIds) > 0:
            q = pinocchio.neutral(self.pinocchioModel)
            self.pinocchioModel = pinocchio.buildReducedModel(
                self.pinocchioModel, jointIds, q)
            self.pinocchioData = pinocchio.Data(self.pinocchioModel)

    def loadModelFromString(self,
                            urdfString,
                            rootJointType=pinocchio.JointModelFreeFlyer,
                            removeMimicJoints=True):
        """ Load a URDF model contained in a string
        - param rootJointType: the root joint type. None for no root joint.
        - param removeMimicJoints: if True, all the mimic joints found in the model are removed.
        """
        if rootJointType is None:
            self.pinocchioModel = pinocchio.buildModelFromXML(urdfString)
        else:
            self.pinocchioModel = pinocchio.buildModelFromXML(
                urdfString, rootJointType())
        self.pinocchioData = pinocchio.Data(self.pinocchioModel)
        if removeMimicJoints:
            self._removeMimicJoints(urdfString=urdfString)

    def loadModelFromUrdf(self,
                          urdfPath,
                          urdfDir=None,
                          rootJointType=pinocchio.JointModelFreeFlyer,
                          removeMimicJoints=True):
        """
        Load a model using the pinocchio urdf parser. This parser looks
        for urdf files in which kinematics and dynamics information
        have been added.
        - param urdfPath: a path to the URDF file.
        - param urdfDir: A list of directories. If None, will use ROS_PACKAGE_PATH.
        """
        if urdfPath.startswith("package://"):
            from os import path
            n1 = 10  # len("package://")
            n2 = urdfPath.index(path.sep, n1)
            pkg = urdfPath[n1:n2]
            relpath = urdfPath[n2 + 1:]

            import rospkg
            rospack = rospkg.RosPack()
            abspkg = rospack.get_path(pkg)
            urdfFile = path.join(abspkg, relpath)
        else:
            urdfFile = urdfPath
        if urdfDir is None:
            import os
            urdfDir = os.environ["ROS_PACKAGE_PATH"].split(':')
        if rootJointType is None:
            self.pinocchioModel = pinocchio.buildModelFromUrdf(urdfFile)
        else:
            self.pinocchioModel = pinocchio.buildModelFromUrdf(
                urdfFile, rootJointType())
        self.pinocchioData = pinocchio.Data(self.pinocchioModel)
        if removeMimicJoints:
            self._removeMimicJoints(urdfFile=urdfFile)

    def initializeOpPoints(self):
        for op in self.OperationalPoints:
            self.dynamic.createOpPoint(op, self.OperationalPointsMap[op])

    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 setJointValueInConfig(self, q, jointNames, jointValues):
        """
        q: configuration to update
        jointNames: list of existing joint names in self.pinocchioModel
        jointValues: corresponding joint values.
        """
        model = self.pinocchioModel
        for jn, jv in zip(jointNames, jointValues):
            assert model.existJointName(jn)
            joint = model.joints[model.getJointId(jn)]
            q[joint.idx_q] = jv

    @abstractmethod
    def defineHalfSitting(self, q):
        """
        Define half sitting configuration using the pinocchio Model (i.e.
        with quaternions and not with euler angles).

        method setJointValueInConfig may be usefull to implement this function.
        """
        pass

    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 hasattr(self, 'dynamic'):
            raise RuntimeError("Dynamic robot model must be initialized first")

        if not hasattr(self, 'device') or self.device is None:
            # raise RuntimeError("A device is already defined.")
            self.device = RobotSimu(self.name + '_device')
        self.device.resize(self.dynamic.getDimension())
        """
        Robot timestep
        """
        self.timeStep = self.device.getTimeStep()

        # Compute half sitting configuration
        import numpy
        """
        Half sitting configuration.
        """
        self.halfSitting = pinocchio.neutral(self.pinocchioModel)
        self.defineHalfSitting(self.halfSitting)
        self.halfSitting = numpy.array(
            self.halfSitting[:3].tolist() +
            [0., 0., 0.]  # Replace quaternion by RPY.
            + self.halfSitting[7:].tolist())
        assert self.halfSitting.shape[0] == self.dynamic.getDimension()

        # Set the device limits.
        def get(s):
            s.recompute(0)
            return s.value

        def opposite(v):
            return [-x for x in v]

        self.dynamic.add_signals()
        self.device.setPositionBounds(get(self.dynamic.lowerJl),
                                      get(self.dynamic.upperJl))
        self.device.setVelocityBounds(-get(self.dynamic.upperVl),
                                      get(self.dynamic.upperVl))
        self.device.setTorqueBounds(-get(self.dynamic.upperTl),
                                    get(self.dynamic.upperTl))

        # 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 = numpy.zeros([
                self.dimension,
            ])

        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 = numpy.zeros([
                self.dimension,
            ])

    def addTrace(self, entityName, signalName):
        if self.tracer:
            self.autoRecomputedSignals.append('{0}.{1}'.format(
                entityName, signalName))
            addTrace(self, self.tracer, entityName, signalName)

    def initializeTracer(self):
        if not self.tracer:
            self.tracer = TracerRealTime('trace')
            self.tracer.setBufferSize(self.tracerSize)
            self.tracer.open('/tmp/', 'dg_', '.dat')
            # Recompute trace.triger at each iteration to enable tracing.
            self.device.after.addSignal('{0}.triger'.format(self.tracer.name))

    def traceDefaultSignals(self):
        # Geometry / operational points
        for s in self.OperationalPoints + self.tracedSignals['dynamic']:
            self.addTrace(self.dynamic.name, s)

        # Geometry / frames
        for (frameName, _, _) in self.AdditionalFrames:
            for s in ['position', 'jacobian']:
                self.addTrace(self.frames[frameName].name, s)

        # Device
        for s in self.tracedSignals['device']:
            self.addTrace(self.device.name, s)
        if type(self.device) != RobotSimu:
            self.addTrace(self.device.name, 'robotState')

        # Misc
        if self.enableVelocityDerivator:
            self.addTrace(self.velocityDerivator.name, 'sout')
        if self.enableAccelerationDerivator:
            self.addTrace(self.accelerationDerivator.name, 'sout')

    def __init__(self, name, tracer=None):
        self._initialize()

        self.name = name

        # Initialize tracer if necessary.
        if tracer:
            self.tracer = tracer

    def __del__(self):
        if self.tracer:
            self.stopTracer()

    def startTracer(self):
        """
        Start the tracer if it does not already been stopped.
        """
        if self.tracer:
            self.tracer.start()

    def stopTracer(self):
        """
        Stop and destroy tracer.
        """
        if self.tracer:
            self.tracer.dump()
            self.tracer.stop()
            self.tracer.close()
            self.tracer.clear()
            for s in self.autoRecomputedSignals:
                self.device.after.rmSignal(s)
            self.tracer = None

    def reset(self, posture=None):
        """
        Restart the control from another position.

        This method has not been extensively tested and
        should be used carefully.

        In particular, tasks should be removed from the
        solver before attempting a reset.
        """
        if not posture:
            posture = self.halfSitting
        self.device.set(posture)

        self.dynamic.com.recompute(self.device.state.time + 1)
        self.dynamic.Jcom.recompute(self.device.state.time + 1)

        for op in self.OperationalPoints:
            self.dynamic.signal(self.OperationalPointsMap[op]).recompute(
                self.device.state.time + 1)
            self.dynamic.signal('J' + self.OperationalPointsMap[op]).recompute(
                self.device.state.time + 1)
예제 #21
0
파일: factory.py 프로젝트: nim65s/sot-hpp
class Factory(GraphFactoryAbstract):
    class State:
        def __init__(self, tasks, grasps, factory):
            self.name = factory._stateName(grasps)
            self.grasps = grasps
            self.manifold = Manifold()

            objectsAlreadyGrasped = {}

            for ig, ih in enumerate(grasps):
                if ih is not None:
                    # Add task gripper_close
                    self.manifold += tasks.g(factory.grippers[ig],
                                             factory.handles[ih],
                                             'gripper_close')
                    otherGrasp = objectsAlreadyGrasped.get(
                        factory.objectFromHandle[ih])
                    self.manifold += tasks.g(factory.grippers[ig],
                                             factory.handles[ih], 'grasp',
                                             otherGrasp)
                    objectsAlreadyGrasped[
                        factory.objectFromHandle[ih]] = tasks.g(
                            factory.grippers[ig], factory.handles[ih], 'grasp',
                            otherGrasp)
                else:
                    # Add task gripper_open
                    self.manifold += tasks.g(factory.grippers[ig], None,
                                             'gripper_open')

    def __init__(self, supervisor):
        super(Factory, self).__init__()
        self.tasks = TaskFactory(self)
        self.hpTasks = supervisor.hpTasks
        self.lpTasks = supervisor.lpTasks
        self.affordances = dict()
        self.sots = dict()
        self.postActions = dict()
        self.preActions = dict()

        self.timers = {}
        self.supervisor = supervisor

        self.addTimerToSotControl = False

    def _newSoT(self, name):
        sot = SOT(name)
        sot.setSize(self.sotrobot.dynamic.getDimension())
        sot.damping.value = 0.001

        if self.addTimerToSotControl:
            from .tools import insertTimerOnOutput
            self.timers[name] = insertTimerOnOutput(sot.control, "vector")
            self.SoTtracer.add(self.timers[name].name + ".timer",
                               str(len(self.timerIds)) + ".timer")
            self.timerIds.append(name)
        return sot

    def addAffordance(self, aff):
        assert isinstance(aff, Affordance)
        self.affordances[(aff.gripper, aff.handle)] = aff

    def generate(self):
        if self.addTimerToSotControl:
            # init tracer
            from dynamic_graph.tracer_real_time import TracerRealTime
            self.SoTtracer = TracerRealTime("tracer_of_timers")
            self.timerIds = []
            self.SoTtracer.setBufferSize(10 * 1048576)  # 10 Mo
            self.SoTtracer.open("/tmp", "sot-control-trace", ".txt")
            self.sotrobot.device.after.addSignal("tracer_of_timers.triger")
            self.supervisor.SoTtracer = self.SoTtracer
            self.supervisor.SoTtimerIds = self.timerIds
        super(Factory, self).generate()

        self.supervisor.sots = self.sots
        self.supervisor.grasps = {(gh, w): t
                                  for gh, ts in self.tasks._grasp.items()
                                  for w, t in ts.items()}
        self.supervisor.hpTasks = self.hpTasks
        self.supervisor.lpTasks = self.lpTasks
        self.supervisor.postActions = self.postActions
        self.supervisor.preActions = self.preActions
        self.supervisor.SoTtimers = self.timers

    def setupFrames(self, srdfGrippers, srdfHandles, sotrobot):
        self.sotrobot = sotrobot

        self.grippersIdx = {g: i for i, g in enumerate(self.grippers)}
        self.handlesIdx = {h: i for i, h in enumerate(self.handles)}

        self.gripperFrames = {
            g: OpFrame(srdfGrippers[g], sotrobot.dynamic.model)
            for g in self.grippers
        }
        self.handleFrames = {h: OpFrame(srdfHandles[h]) for h in self.handles}

    def makeState(self, grasps, priority):
        # Nothing to do here
        return Factory.State(self.tasks, grasps, self)

    def makeLoopTransition(self, state):
        n = self._loopTransitionName(state.grasps)
        sot = self._newSoT('sot_' + n)

        self.hpTasks.pushTo(sot)
        state.manifold.pushTo(sot)
        self.lpTasks.pushTo(sot)

        self.sots[n] = sot

    def makeTransition(self, stateFrom, stateTo, ig):
        sf = stateFrom
        st = stateTo
        names = self._transitionNames(sf, st, ig)

        print "names: {}".format(names)

        iobj = self.objectFromHandle[st.grasps[ig]]
        obj = self.objects[iobj]
        noPlace = self._isObjectGrasped(sf.grasps, iobj)

        print "iobj, obj, noPlace: {}, {}, {}".format(iobj, obj, noPlace)

        # The different cases:
        pregrasp = True
        intersec = not noPlace
        preplace = not noPlace

        # Start here
        nWaypoints = pregrasp + intersec + preplace
        nTransitions = 1 + nWaypoints

        # Link waypoints
        transitions = names[:]
        assert nWaypoints > 0
        M = 1 + pregrasp
        sots = []
        for i in range(nTransitions):
            ns = ("{0}_{1}{2}".format(names[0], i, i + 1),
                  "{0}_{2}{1}".format(names[1], i, i + 1))

            for n in ns:
                s = self._newSoT('sot_' + n)
                self.hpTasks.pushTo(s)

                #if pregrasp and i == 1:
                # Add pregrasp task
                #self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'pregrasp').pushTo (s)
                if i < M: sf.manifold.pushTo(s)
                else: st.manifold.pushTo(s)

                self.lpTasks.pushTo(s)
                self.sots[n] = s
                sots.append(n)

        key = sots[2 * (M - 1)]
        states = (st.name, names[0] + "_intersec")

        sot = self._newSoT("postAction_" + key)
        self.hpTasks.pushTo(sot)
        # self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot)
        st.manifold.pushTo(sot)
        self.lpTasks.pushTo(sot)
        # print sot

        # TODO one post action is missing
        if not self.postActions.has_key(key):
            self.postActions[key] = dict()
        for n in states:
            self.postActions[key][n] = sot

        key = sots[2 * (M - 1) + 1]
        states = (st.name, names[0] + "_intersec")

        sot = self._newSoT("preAction_" + key)
        self.hpTasks.pushTo(sot)
        # self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot)
        sf.manifold.pushTo(sot)
        self.lpTasks.pushTo(sot)
        # print sot

        self.preActions[key] = sot
        self.preActions[sots[2 * (M - 1)]] = sot
plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
plug(robot.com_admittance_control.dcomRef, robot.taskCom.featureDes.errordotIN)
robot.taskCom.task.controlGain.value = 0
robot.taskCom.task.setWithDerivative(True)

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

robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.device.control.recompute(0)

# --- TRACER
robot.tracer = TracerRealTime("zmp_tracer")
robot.tracer.setBufferSize(80 * (2**20))
robot.tracer.open('/tmp', 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
robot.device.after.addSignal('{0}.zmpRef'.format(robot.dcm_control.name))
robot.device.after.addSignal('{0}.zmp'.format(robot.dynamic.name))  # why needed?
robot.device.after.addSignal('{0}.comRef'.format(robot.com_admittance_control.name))  # why needed?

addTrace(robot.tracer, robot.dynamic, 'zmp')
addTrace(robot.tracer, robot.dcm_control, 'zmpRef')
addTrace(robot.tracer, robot.estimator, 'dcm')
addTrace(robot.tracer, robot.dynamic, 'com')
addTrace(robot.tracer, robot.com_admittance_control, 'comRef')

# SIMULATION
예제 #23
0
def init_online_walking(robot):
    # 09.04.20 est a 100 dans dcmZmpControl_file, used to be 10
    cm_conf.CTRL_MAX = 1000.0  # temporary hack
    dt = robot.device.getTimeStep()
    robot.timeStep = dt

    # --- Pendulum parameters
    robot_name = 'robot'
    robot.dynamic.com.recompute(0)
    robotDim = robot.dynamic.getDimension()
    mass = robot.dynamic.data.mass[0]
    h = robot.dynamic.com.value[2]
    g = 9.81
    omega = sqrt(g / h)

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

    # --- Initial feet and waist
    robot.dynamic.createOpPoint('LF', robot.OperationalPointsMap['left-ankle'])
    robot.dynamic.createOpPoint('RF',
                                robot.OperationalPointsMap['right-ankle'])
    robot.dynamic.createOpPoint('WT', robot.OperationalPointsMap['waist'])
    robot.dynamic.LF.recompute(0)
    robot.dynamic.RF.recompute(0)
    robot.dynamic.WT.recompute(0)

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

    rospack = RosPack()

    # -------------------------- PATTERN GENERATOR --------------------------

    robot.pg = PatternGenerator('pg')

    # MODIFIED WITH MY PATHS
    talos_data_folder = rospack.get_path('talos_data')
    robot.pg.setURDFpath(talos_data_folder + '/urdf/talos_reduced_wpg.urdf')
    robot.pg.setSRDFpath(talos_data_folder + '/srdf/talos_wpg.srdf')
    ## END MODIFIED

    robot.pg.buildModel()

    robot.pg.parseCmd(":samplingperiod 0.005")
    robot.pg.parseCmd(":previewcontroltime 1.6")
    robot.pg.parseCmd(":omega 0.0")
    robot.pg.parseCmd(':stepheight 0.05')
    robot.pg.parseCmd(':doublesupporttime 0.2')
    robot.pg.parseCmd(':singlesupporttime 1.0')
    robot.pg.parseCmd(":armparameters 0.5")
    robot.pg.parseCmd(":LimitsFeasibility 0.0")
    robot.pg.parseCmd(":ZMPShiftParameters 0.015 0.015 0.015 0.015")
    robot.pg.parseCmd(":TimeDistributeParameters 2.0 3.5 1.7 3.0")
    robot.pg.parseCmd(":UpperBodyMotionParameters -0.1 -1.0 0.0")
    robot.pg.parseCmd(":comheight 0.876681")
    robot.pg.parseCmd(":setVelReference  0.1 0.0 0.0")

    robot.pg.parseCmd(":SetAlgoForZmpTrajectory Naveau")

    plug(robot.dynamic.position, robot.pg.position)
    plug(robot.dynamic.com, robot.pg.com)
    #plug(robot.dynamic.com, robot.pg.comStateSIN)
    plug(robot.dynamic.LF, robot.pg.leftfootcurrentpos)
    plug(robot.dynamic.RF, robot.pg.rightfootcurrentpos)
    robotDim = len(robot.dynamic.velocity.value)
    robot.pg.motorcontrol.value = robotDim * (0, )
    robot.pg.zmppreviouscontroller.value = (0, 0, 0)

    robot.pg.initState()

    robot.pg.parseCmd(':setDSFeetDistance 0.162')

    robot.pg.parseCmd(':NaveauOnline')
    robot.pg.parseCmd(':numberstepsbeforestop 2')
    robot.pg.parseCmd(':setfeetconstraint XY 0.091 0.0489')

    robot.pg.parseCmd(':deleteallobstacles')
    robot.pg.parseCmd(':feedBackControl false')
    robot.pg.parseCmd(':useDynamicFilter true')

    robot.pg.velocitydes.value = (0.1, 0.0, 0.0)  # DEFAULT VALUE (0.1,0.0,0.0)

    # -------------------------- TRIGGER --------------------------

    robot.triggerPG = BooleanIdentity('triggerPG')
    robot.triggerPG.sin.value = 0
    plug(robot.triggerPG.sout, robot.pg.trigger)

    # --------- Interface with controller entities -------------

    wp = DummyWalkingPatternGenerator('dummy_wp')
    wp.init()
    # #wp.displaySignals()
    wp.omega.value = omega

    # 22.04 after modifying pg.cpp, new way to try and connect the waist
    plug(robot.pg.waistattitudematrixabsolute, wp.waist)
    plug(robot.pg.leftfootref, wp.footLeft)
    plug(robot.pg.rightfootref, wp.footRight)
    plug(robot.pg.comref, wp.com)
    plug(robot.pg.dcomref, wp.vcom)
    plug(robot.pg.ddcomref, wp.acom)

    robot.wp = wp

    # --- Compute the values to use them in initialization
    robot.wp.comDes.recompute(0)
    robot.wp.dcmDes.recompute(0)
    robot.wp.zmpDes.recompute(0)

    ## END ADDED

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

    # --- Base Estimation
    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.m2qLF = MatrixHomoToPoseQuaternion('m2qLF')
    plug(robot.dynamic.LF, robot.m2qLF.sin)
    plug(robot.m2qLF.sout, robot.base_estimator.lf_ref_xyzquat)
    robot.m2qRF = MatrixHomoToPoseQuaternion('m2qRF')
    plug(robot.dynamic.RF, robot.m2qRF.sin)
    plug(robot.m2qRF.sout, robot.base_estimator.rf_ref_xyzquat)

    # --- 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)
    robot.cdc_estimator = cdc_estimator

    # --- DCM Estimation
    estimator = DummyDcmEstimator("dummy")
    estimator.omega.value = omega
    estimator.mass.value = 1.0
    plug(robot.cdc_estimator.c, estimator.com)
    plug(robot.cdc_estimator.dc, estimator.momenta)
    estimator.init()
    robot.estimator = estimator

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

    # --- ZMP estimation
    zmp_estimator = SimpleZmpEstimator("zmpEst")
    robot.rdynamic.createOpPoint('sole_LF', 'left_sole_link')
    robot.rdynamic.createOpPoint('sole_RF', 'right_sole_link')
    plug(robot.rdynamic.sole_LF, zmp_estimator.poseLeft)
    plug(robot.rdynamic.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()
    robot.zmp_estimator = zmp_estimator

    # -------------------------- ADMITTANCE CONTROL --------------------------

    # --- DCM controller
    Kp_dcm = [8.0] * 3
    Ki_dcm = [0.0, 0.0, 0.0]  # zero (to be set later)
    gamma_dcm = 0.2

    dcm_controller = DcmController("dcmCtrl")

    dcm_controller.Kp.value = Kp_dcm
    dcm_controller.Ki.value = Ki_dcm
    dcm_controller.decayFactor.value = gamma_dcm
    dcm_controller.mass.value = mass
    dcm_controller.omega.value = omega

    plug(robot.cdc_estimator.c, dcm_controller.com)
    plug(robot.estimator.dcm, dcm_controller.dcm)

    plug(robot.wp.zmpDes, dcm_controller.zmpDes)
    plug(robot.wp.dcmDes, dcm_controller.dcmDes)

    dcm_controller.init(dt)

    robot.dcm_control = dcm_controller

    Ki_dcm = [1.0, 1.0, 1.0]  # this value is employed later

    # --- CoM admittance controller
    Kp_adm = [0.0, 0.0, 0.0]  # zero (to be set later)

    com_admittance_control = ComAdmittanceController("comAdmCtrl")
    com_admittance_control.Kp.value = Kp_adm
    plug(robot.zmp_estimator.zmp, com_admittance_control.zmp)
    com_admittance_control.zmpDes.value = robot.wp.zmpDes.value
    # should be plugged to robot.dcm_control.zmpRef
    plug(robot.wp.acomDes, com_admittance_control.ddcomDes)

    com_admittance_control.init(dt)
    com_admittance_control.setState(robot.wp.comDes.value, [0.0, 0.0, 0.0])

    robot.com_admittance_control = com_admittance_control

    Kp_adm = [15.0, 15.0, 0.0]  # this value is employed later

    # --- Control Manager
    robot.cm = create_ctrl_manager(cm_conf, dt, robot_name='robot')
    robot.cm.addCtrlMode('sot_input')
    robot.cm.setCtrlMode('all', 'sot_input')
    robot.cm.addEmergencyStopSIN('zmp')

    # -------------------------- SOT CONTROL --------------------------

    # --- Upper body
    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(18, True)
    robot.taskUpperBody.feature.selectDof(19, True)
    robot.taskUpperBody.feature.selectDof(20, True)
    robot.taskUpperBody.feature.selectDof(21, True)
    robot.taskUpperBody.feature.selectDof(22, True)
    robot.taskUpperBody.feature.selectDof(23, True)
    robot.taskUpperBody.feature.selectDof(24, True)
    robot.taskUpperBody.feature.selectDof(25, True)
    robot.taskUpperBody.feature.selectDof(26, True)
    robot.taskUpperBody.feature.selectDof(27, True)
    robot.taskUpperBody.feature.selectDof(28, True)
    robot.taskUpperBody.feature.selectDof(29, True)
    robot.taskUpperBody.feature.selectDof(30, True)
    robot.taskUpperBody.feature.selectDof(31, True)
    robot.taskUpperBody.feature.selectDof(32, True)
    robot.taskUpperBody.feature.selectDof(33, True)
    robot.taskUpperBody.feature.selectDof(34, True)
    robot.taskUpperBody.feature.selectDof(35, True)
    robot.taskUpperBody.feature.selectDof(36, True)
    robot.taskUpperBody.feature.selectDof(37, True)

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

    # --- CONTACTS
    robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
                                     robot.OperationalPointsMap['left-ankle'])
    robot.contactLF.feature.frame('desired')
    robot.contactLF.gain.setConstant(300)
    plug(robot.wp.footLeftDes, robot.contactLF.featureDes.position)  #.errorIN?
    locals()['contactLF'] = robot.contactLF

    robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
                                     robot.OperationalPointsMap['right-ankle'])
    robot.contactRF.feature.frame('desired')
    robot.contactRF.gain.setConstant(300)
    plug(robot.wp.footRightDes,
         robot.contactRF.featureDes.position)  #.errorIN?
    locals()['contactRF'] = robot.contactRF

    # --- COM height
    robot.taskComH = MetaTaskKineCom(robot.dynamic, name='comH')
    plug(robot.wp.comDes, robot.taskComH.featureDes.errorIN)
    robot.taskComH.task.controlGain.value = 100.
    robot.taskComH.feature.selec.value = '100'

    # --- COM
    robot.taskCom = MetaTaskKineCom(robot.dynamic)
    plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
    plug(robot.com_admittance_control.dcomRef,
         robot.taskCom.featureDes.errordotIN)
    robot.taskCom.task.controlGain.value = 0
    robot.taskCom.task.setWithDerivative(True)
    robot.taskCom.feature.selec.value = '011'

    # --- Waist

    robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT',
                                     robot.OperationalPointsMap['waist'])
    robot.keepWaist.feature.frame('desired')
    robot.keepWaist.gain.setConstant(300)
    plug(robot.wp.waistDes, robot.keepWaist.featureDes.position)  #de base
    robot.keepWaist.feature.selec.value = '111000'
    locals()['keepWaist'] = robot.keepWaist

    # --- SOT solver
    robot.sot = SOT('sot')
    robot.sot.setSize(robot.dynamic.getDimension())

    # --- Plug SOT control to device through control manager
    plug(robot.sot.control, robot.cm.ctrl_sot_input)
    plug(robot.cm.u_safe, robot.device.control)

    robot.sot.push(robot.taskUpperBody.name)
    robot.sot.push(robot.contactRF.task.name)
    robot.sot.push(robot.contactLF.task.name)
    robot.sot.push(robot.taskComH.task.name)
    robot.sot.push(robot.taskCom.task.name)
    robot.sot.push(robot.keepWaist.task.name)

    # --- Fix robot.dynamic inputs
    plug(robot.device.velocity, robot.dynamic.velocity)
    robot.dvdt = Derivator_of_Vector("dv_dt")
    robot.dvdt.dt.value = dt
    plug(robot.device.velocity, robot.dvdt.sin)
    plug(robot.dvdt.sout, robot.dynamic.acceleration)

    # -------------------------- PLOTS --------------------------

    # --- ROS PUBLISHER

    ## THIS PARAGRAPH QUITE DIFFERENT, TO CHECK

    robot.publisher = create_rospublish(robot, 'robot_publisher')

    ## ADDED
    create_topic(robot.publisher,
                 robot.pg,
                 'comref',
                 robot=robot,
                 data_type='vector')  # desired CoM
    create_topic(robot.publisher,
                 robot.pg,
                 'dcomref',
                 robot=robot,
                 data_type='vector')

    create_topic(robot.publisher,
                 robot.wp,
                 'waist',
                 robot=robot,
                 data_type='matrixHomo')
    create_topic(robot.publisher,
                 robot.keepWaist.featureDes,
                 'position',
                 robot=robot,
                 data_type='matrixHomo')
    create_topic(robot.publisher,
                 robot.dynamic,
                 'WT',
                 robot=robot,
                 data_type='matrixHomo')
    create_topic(robot.publisher,
                 robot.pg,
                 'waistattitudematrixabsolute',
                 robot=robot,
                 data_type='matrixHomo')  ## que font ces lignes exactement ??

    create_topic(robot.publisher,
                 robot.pg,
                 'leftfootref',
                 robot=robot,
                 data_type='matrixHomo')
    create_topic(robot.publisher,
                 robot.wp,
                 'footLeft',
                 robot=robot,
                 data_type='matrixHomo')
    create_topic(robot.publisher,
                 robot.pg,
                 'rightfootref',
                 robot=robot,
                 data_type='matrixHomo')
    create_topic(robot.publisher,
                 robot.wp,
                 'footRight',
                 robot=robot,
                 data_type='matrixHomo')

    ## --- TRACER
    robot.tracer = TracerRealTime("com_tracer")
    robot.tracer.setBufferSize(80 * (2**20))
    robot.tracer.open('/tmp', 'dg_', '.dat')

    robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))

    addTrace(robot.tracer, robot.pg, 'waistattitudeabsolute')
    # fin

    addTrace(robot.tracer, robot.wp, 'comDes')  # desired CoM

    addTrace(robot.tracer, robot.cdc_estimator, 'c')  # estimated CoM
    addTrace(robot.tracer, robot.cdc_estimator, 'dc')  # estimated CoM velocity

    addTrace(robot.tracer, robot.pg, 'comref')
    addTrace(robot.tracer, robot.pg, 'dcomref')
    addTrace(robot.tracer, robot.pg, 'ddcomref')

    addTrace(robot.tracer, robot.pg, 'rightfootref')
    addTrace(robot.tracer, robot.pg, 'leftfootref')

    addTrace(robot.tracer, robot.pg, 'rightfootcontact')
    addTrace(robot.tracer, robot.pg, 'leftfootcontact')
    addTrace(robot.tracer, robot.pg, 'SupportFoot')

    addTrace(robot.tracer, robot.dynamic, 'com')  # resulting SOT CoM
    addTrace(robot.tracer, robot.dynamic, 'LF')  # left foot
    addTrace(robot.tracer, robot.dynamic, 'RF')  # right foot

    robot.tracer.start()
class LegsFollowerGraph(object):

    legsFollower = None

    postureTask = None
    postureFeature = None
    postureFeatureDes = None
    postureError = None

    legsTask = None
    legsFeature = None
    legsFeatureDes = None
    legsError = None

    waistTask = None
    waistFeature = None
    waistFeatureDes = None
    waistError = None

    trace = None

    def __init__(self, robot, solver, trace = None, postureTaskDofs = None):
        print("Constructor of LegsFollower Graph")
        self.robot = robot
        self.solver = solver
	self.legsFollower = LegsFollower('legs-follower')
        self.statelength = len(robot.device.state.value)

 	# Initialize the posture task.
	print("Posture Task")
        self.postureTaskDofs = postureTaskDofs
        if not self.postureTaskDofs:
            self.postureTaskDofs = []
            for i in xrange(len(robot.halfSitting) - 6):
                # Disable legs dofs.
                if i < 12: #FIXME: not generic enough
                    self.postureTaskDofs.append((i + 6, False))
                else:
                    self.postureTaskDofs.append((i + 6, True))
        
        # This part is taken from feet_follower_graph
        self.postureTask = Task(self.robot.name + '_posture')
        
        self.postureFeature = FeaturePosture(self.robot.name + '_postureFeature')
        plug(self.robot.device.state, self.postureFeature.state)

        posture = list(self.robot.halfSitting)
        self.postureFeature.setPosture(tuple(posture))
        for (dof, isEnabled) in self.postureTaskDofs:
            self.postureFeature.selectDof(dof, isEnabled)
        self.postureTask.add(self.postureFeature.name)
        self.postureTask.controlGain.value = 2.

	# Initialize the waist follower task.
	print("Waist Task")
        self.robot.features['waist'].selec.value = '111111'
        plug(self.legsFollower.waist, self.robot.features['waist'].reference)
        self.robot.tasks['waist'].controlGain.value = 1.

	# Initialize the legs follower task.
	print("Legs Task")
        self.legsTask = Task(self.robot.name + '_legs')
        self.legsFeature = FeatureGeneric(self.robot.name + '_legsFeature')
        legsFeatureDesName = self.robot.name + '_legsFeatureDes'
        self.legsFeatureDes = FeatureGeneric(legsFeatureDesName)
        self.legsError = LegsError('LegsError')
        plug(self.robot.device.state, self.legsError.state)

        # self.legsFeatureDes.errorIN.value = self.legsFollower.ldof.value        
        plug(self.legsFollower.ldof,self.legsFeatureDes.errorIN)
        self.legsFeature.jacobianIN.value = self.legsJacobian()

        self.legsFeature.setReference(legsFeatureDesName)
        plug(self.legsError.error, self.legsFeature.errorIN)            

        self.legsTask.add(self.legsFeature.name)
        self.legsTask.controlGain.value = 5.

	#CoM task
        print("Com Task")
        print (0., 0., self.robot.dynamic.com.value[2])
	self.robot.comTask.controlGain.value = 50.
        self.robot.featureComDes.errorIN.value =  (0., 0., self.robot.dynamic.com.value[2])
        self.robot.featureCom.selec.value = '111'
	plug(self.legsFollower.com, self.robot.featureComDes.errorIN)

        # Plug the legs follower zmp output signals.
        plug(self.legsFollower.zmp, self.robot.device.zmp)


	solver.sot.remove(self.robot.comTask.name)

	print("Push in solver.")
        solver.sot.push(self.legsTask.name)
        solver.sot.push(self.postureTask.name)
	solver.sot.push(self.robot.tasks['waist'].name)
        solver.sot.push(self.robot.comTask.name)
        
        solver.sot.remove(self.robot.tasks['left-ankle'].name)
	solver.sot.remove(self.robot.tasks['right-ankle'].name)


	print solver.sot.display()

        print("Tasks added in solver.\n")
	print("Command are : \n - f.plug()\n - f.plugViewer()\n - f.plugPlanner()\n"
              " - f.plugPlannerWithoutMocap()\n - f.start()\n - f.stop()\n - f.readMocap()\n")


    def legsJacobian(self):
        j = []
        for i in xrange(12):
            j.append(oneVector(6+i,self.statelength)) 
        return tuple(j)

    def waistJacobian(self):
        j = []
        for i in xrange(6):
            j.append(oneVector(i,self.statelength)) 
        return tuple(j)

    def postureJacobian(self):
        j = []
        for i in xrange(self.statelength):
            if i >= 6 + 2 * 6:
                j.append(oneVector(i))
            if i == 3 or i == 4:
                j.append(zeroVector())
        return tuple(j)

    def computeDesiredValue(self):
        e = self.robot.halfSitting
        #e = halfSitting
        e_ = [e[3], e[4]]
        offset = 6 + 2 * 6
        for i in xrange(len(e) - offset):
            e_.append(e[offset + i])
        return tuple(e_)
    
    def canStart(self):
        securityThreshold = 1e-3
        return (self.postureTask.error.value <=
                (securityThreshold,) * len(self.postureTask.error.value))

    def setupTrace(self):
	self.trace = TracerRealTime('trace')
	self.trace.setBufferSize(2**20)
	self.trace.open('/tmp/','legs_follower_','.dat')
	
	self.trace.add('legs-follower.com', 'com')
	self.trace.add('legs-follower.zmp', 'zmp')
	self.trace.add('legs-follower.ldof', 'ldof')
	self.trace.add('legs-follower.waist', 'waist')
	self.trace.add(self.robot.device.name + '.state', 'state')
	self.trace.add(self.legsTask.name + '.error', 'errorLegs')
        self.trace.add(self.robot.comTask.name + '.error', 'errorCom')

        #self.trace.add('legs-follower.outputStart','start')
        #self.trace.add('legs-follower.outputYaw','yaw')
        self.trace.add('corba.planner_steps','steps')
        self.trace.add('corba.planner_outputGoal','goal')
        self.trace.add('corba.waist','waistMocap')
	self.trace.add('corba.left-foot','footMocap')
        self.trace.add('corba.table','tableMocap')
        self.trace.add('corba.bar','barMocap')
        self.trace.add('corba.chair','chairMocap')
	self.trace.add('corba.helmet','helmetMocap')
	self.trace.add('corba.planner_outputObs','obstacles')

        self.trace.add(self.robot.dynamic.name + '.left-ankle',
                       self.robot.dynamic.name + '-left-ankle')
        self.trace.add(self.robot.dynamic.name + '.right-ankle',
                       self.robot.dynamic.name + '-right-ankle')


	# Recompute trace.triger at each iteration to enable tracing.
	self.robot.device.after.addSignal('legs-follower.zmp')
	self.robot.device.after.addSignal('legs-follower.outputStart')
	self.robot.device.after.addSignal('legs-follower.outputYaw')
	self.robot.device.after.addSignal('corba.planner_steps')
	self.robot.device.after.addSignal('corba.planner_outputGoal')
	self.robot.device.after.addSignal('corba.waist')
	self.robot.device.after.addSignal('corba.left-foot')
	self.robot.device.after.addSignal('corba.table')
	self.robot.device.after.addSignal('corba.bar')
	self.robot.device.after.addSignal('corba.chair')
	self.robot.device.after.addSignal('corba.helmet')
	self.robot.device.after.addSignal('corba.planner_outputObs')
        self.robot.device.after.addSignal(self.robot.dynamic.name + '.left-ankle')
	self.robot.device.after.addSignal(self.robot.dynamic.name + '.right-ankle')
	self.robot.device.after.addSignal('trace.triger')
	return

    def plugPlanner(self):
        print("Plug planner.")
	plug(corba.planner_radQ, self.legsFollower.inputRef)
	plug(self.legsFollower.outputStart, corba.planner_inputStart)
	plug(corba.waistTimestamp, corba.planner_timestamp)
	plug(corba.table, corba.planner_table)
	plug(corba.chair, corba.planner_chair)
	plug(corba.bar, corba.planner_bar)
	plug(corba.waist, corba.planner_waist)
	plug(corba.helmet, corba.planner_inputGoal)
        plug(corba.torus1, corba.planner_torus1)
	plug(corba.torus2, corba.planner_torus2)
        plug(corba.signal('left-foot'), corba.planner_foot)
        plug(corba.signal('left-footTimestamp'), corba.planner_footTimestamp)
	return

    def plugPlannerWithoutMocap(self):
        print("Plug planner without mocap.")
	plug(corba.planner_radQ, self.legsFollower.inputRef)
	plug(self.legsFollower.outputStart, corba.planner_inputStart)
	return

    def plugViewer(self):
        print("Plug viewer.")
	plug(self.legsFollower.ldof, corba.viewer_Ldof)
	plug(self.legsFollower.outputStart, corba.viewer_Start)
	plug(self.legsFollower.com, corba.viewer_Com)
	plug(self.legsFollower.outputYaw, corba.viewer_Yaw)
	plug(corba.planner_steps, corba.viewer_Steps)
	plug(corba.planner_outputGoal, corba.viewer_Goal)
	plug(corba.table, corba.viewer_Table)
	plug(corba.chair, corba.viewer_Chair)
	plug(corba.bar, corba.viewer_Bar)
        plug(corba.torus1, corba.viewer_Torus1)
	plug(corba.torus2, corba.viewer_Torus2)
	plug(corba.waist, corba.viewer_Waist)
	plug(corba.planner_outputLabyrinth, corba.viewer_Labyrinth)
	plug(corba.planner_outputObs, corba.viewer_Obs)
	return

    def plug(self):
	self.plugPlanner()
	self.plugViewer()
	return

    def readMocap(self):
	print "Table : " 
	print corba.table.value
	print "Waist : " 
	if len(corba.waist.value)<3:
	    corba.waist.value = (0,0,0)
	print corba.waist.value
	print "Helmet : " 
	print corba.helmet.value
	return;

    def start(self):
        if not self.canStart():
            print("Robot has not yet converged to the initial position,"
                  " please wait and try again.")
            return

        print("Start.")
	self.postureTask.controlGain.value = 180.
        #self.waistTask.controlGain.value = 90.
	self.legsTask.controlGain.value = 180.
	self.robot.comTask.controlGain.value = 180.
	self.robot.tasks['waist'].controlGain.value = 45.

	self.setupTrace()
	self.trace.start()
        self.legsFollower.start()
	return

    def stop(self):
	self.legsFollower.stop()
	self.trace.dump()
	return
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')
create_topic(robot.publisher, robot.device, 'motorcontrol', 'motorcontrol', robot=robot, data_type='vector')
create_topic(robot.publisher, robot.device, 'robotState', 'robotState', robot=robot, data_type='vector')

# # --- TRACER
robot.tracer = TracerRealTime("tau_tracer")
robot.tracer.setBufferSize(80*(2**20))
robot.tracer.open('/tmp','dg_','.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))

addTrace(robot.tracer, robot.inv_dyn, 'tau_des')
addTrace(robot.tracer, robot.inv_dyn, 'q_des')
addTrace(robot.tracer, robot.inv_dyn, 'v_des')
addTrace(robot.tracer, robot.inv_dyn, 'dv_des')
addTrace(robot.tracer, robot.errorPoseTSID, 'sout')
addTrace(robot.tracer, robot.errorComTSID, 'sout')
addTrace(robot.tracer, robot.device, 'robotState')
addTrace(robot.tracer, robot.device, 'motorcontrol')

robot.tracer.start()
class AbstractHumanoidRobot (object):
    """
    This class instantiates all the entities required to get a consistent
    representation of a humanoid robot, mainly:
      - device : to integrate velocities into angular control,
      - dynamic: to compute forward geometry and kinematics,
      - zmpFromForces: to compute ZMP force foot force sensors,
      - stabilizer: to stabilize balanced motions

    Operational points are stored into 'OperationalPoints' list. Some of them
    are also accessible directly as attributes:
      - leftWrist,
      - rightWrist,
      - leftAnkle,
      - rightAnkle,
      - Gaze.

    Operational points are mapped to the actual joints in the robot model
    via 'OperationalPointsMap' dictionary.
    This attribute *must* be defined in the subclasses

    Other attributes require to be defined:
        - halfSitting: half-sitting position is the robot initial pose.
            This attribute *must* be defined in subclasses.

        - dynamic: The robot dynamic model.

        - device: The device that integrates the dynamic equation, namely
            the real robot or
            a simulator

        - dimension: The configuration size.
    """


    def _initialize (self):
        self.OperationalPoints = ['left-wrist', 'right-wrist',
                             'left-ankle', 'right-ankle',
                             'gaze']


        """
        Operational points are specific interesting points of the robot
        used to control it.

        When an operational point is defined, signals corresponding to the
        point position and jacobian are created.

        For instance if creating an operational point for the left-wrist,
        the associated signals will be called "left-wrist" and
        "Jleft-wrist" for respectively the position and the jacobian.
        """

        self.AdditionalFrames = []
        """
        Additional frames are frames which are defined w.r.t an operational point
        and provides an interesting transformation.

        It can be used, for instance, to store the sensor location.

        The contained elements must be triplets matching:
        - additional frame name,
        - transformation w.r.t to the operational point,
        - operational point file.
        """


        self.frames = dict()
        """
        Additional frames defined by using OpPointModifier.
        """

        #FIXME: the following options are /not/ independent.
        # zmp requires acceleration which requires velocity.
        """
        Enable velocity computation.
        """
        self.enableVelocityDerivator = False
        """
        Enable acceleration computation.
        """
        self.enableAccelerationDerivator = False
        """
        Enable ZMP computation
        """
        self.enableZmpComputation = False

        """
        Tracer used to log data.
        """
        self.tracer = None

        """
        How much data will be logged.
        """
        self.tracerSize = 2**20

        """
        Automatically recomputed signals through the use
        of device.after.
        This list is maintained in order to clean the
        signal list device.after before exiting.
        """
        self.autoRecomputedSignals = []

        """
        Which signals should be traced.
        """
        self.tracedSignals = {
            'dynamic': ["com", "zmp", "angularmomentum",
                      "position", "velocity", "acceleration"],
            'device': ['zmp', 'control', 'state']
            }


    def help (self):
        print (AbstractHumanoidRobot.__doc__)

    def loadModelFromKxml(self, name, filename):
        """
        Load a model from a kxml file and return the parsed model.
        This uses the Python parser class implement in
        dynamic_graph.sot.dynamics_pinocchio.parser.

        kxml is an extensible file format used by KineoWorks to store
        both the robot mesh and its kinematic chain.

        The parser also imports inertia matrices which is a
        non-standard property.
        """
        model = Parser(name, filename).parse()
        self.setProperties(model)
        return model

    def loadModelFromUrdf(self, name, urdfPath,
                          dynamicType):
        """
        Load a model using the pinocchio urdf parser. This parser looks
        for urdf files in which kinematics and dynamics information
        have been added.

        Additional information are located in two different XML files.
        """
        model = dynamicType(name)
        #TODO: setproperty flags in sot-pinocchio
        #self.setProperties(model)
        model.setFile(urdfPath)
        model.parse()
        return model

    #TODO: put these flags in sot-pinocchio
    #def setProperties(self, model):
    #    model.setProperty('TimeStep', str(self.timeStep))
    #
    #    model.setProperty('ComputeAcceleration', 'false')
    #    model.setProperty('ComputeAccelerationCoM', 'false')
    #    model.setProperty('ComputeBackwardDynamics', 'false')
    #    model.setProperty('ComputeCoM', 'false')
    #    model.setProperty('ComputeMomentum', 'false')
    #    model.setProperty('ComputeSkewCom', 'false')
    #    model.setProperty('ComputeVelocity', 'false')
    #    model.setProperty('ComputeZMP', 'false')
    #    model.setProperty('ComputeAccelerationCoM', 'true')
    #    model.setProperty('ComputeCoM', 'true')
    #    model.setProperty('ComputeVelocity', 'true')
    #    model.setProperty('ComputeZMP', 'true')
    #
    #    if self.enableZmpComputation:
    #        model.setProperty('ComputeBackwardDynamics', 'true')
    #        model.setProperty('ComputeAcceleration', 'true')
    #        model.setProperty('ComputeMomentum', 'true')


    def initializeOpPoints(self):
        for op in self.OperationalPoints:
            self.dynamic.createOpPoint(op, self.OperationalPointsMap[op])

    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 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')

        """
        Robot timestep
        """
        self.timeStep = self.device.getTimeStep()

        # 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.,)

        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.,)

        #self.initializeOpPoints()

        #TODO: hand parameters through srdf --- 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 addTrace(self, entityName, signalName):
        if self.tracer:
            self.autoRecomputedSignals.append(
                '{0}.{1}'.format(entityName, signalName))
            addTrace(self, self.tracer, entityName, signalName)

    def initializeTracer(self):
        if not self.tracer:
            self.tracer = TracerRealTime('trace')
            self.tracer.setBufferSize(self.tracerSize)
            self.tracer.open('/tmp/','dg_','.dat')
            # Recompute trace.triger at each iteration to enable tracing.
            self.device.after.addSignal('{0}.triger'.format(self.tracer.name))

    def traceDefaultSignals (self):
        # Geometry / operational points
        for s in self.OperationalPoints + self.tracedSignals['dynamic']:
            self.addTrace(self.dynamic.name, s)

        # Geometry / frames
        for (frameName, _, _) in self.AdditionalFrames:
            for s in ['position', 'jacobian']:
                self.addTrace(self.frames[frameName].name, s)

        # Device
        for s in self.tracedSignals['device']:
            self.addTrace(self.device.name, s)
        if type(self.device) != RobotSimu:
            self.addTrace(self.device.name, 'robotState')

        # Misc
        if self.enableVelocityDerivator:
            self.addTrace(self.velocityDerivator.name, 'sout')
        if self.enableAccelerationDerivator:
            self.addTrace(self.accelerationDerivator.name, 'sout')


    def __init__(self, name, tracer = None):
        self._initialize()

        self.name = name

        # Initialize tracer if necessary.
        if tracer:
            self.tracer = tracer

    def __del__(self):
        if self.tracer:
            self.stopTracer()

    def startTracer(self):
        """
        Start the tracer if it does not already been stopped.
        """
        if self.tracer:
            self.tracer.start()

    def stopTracer(self):
        """
        Stop and destroy tracer.
        """
        if self.tracer:
            self.tracer.dump()
            self.tracer.stop()
            self.tracer.close()
            self.tracer.clear()
            for s in self.autoRecomputedSignals:
                self.device.after.rmSignal(s)
            self.tracer = None

    def reset(self, posture = None):
        """
        Restart the control from another position.

        This method has not been extensively tested and
        should be used carefully.

        In particular, tasks should be removed from the
        solver before attempting a reset.
        """
        if not posture:
            posture = self.halfSitting
        self.device.set(posture)

        self.dynamic.com.recompute(self.device.state.time+1)
        self.dynamic.Jcom.recompute(self.device.state.time+1)

        for op in self.OperationalPoints:
            self.dynamic.signal(self.OperationalPointsMap[op]).recompute(self.device.state.time+1)
            self.dynamic.signal('J'+self.OperationalPointsMap[op]).recompute(self.device.state.time+1)
def main(robot):
    robot.timeStep = robot.device.getTimeStep()
    dt = robot.timeStep

    # --- COM
    robot.taskCom = MetaTaskKineCom(robot.dynamic)
    robot.dynamic.com.recompute(0)
    robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
    robot.taskCom.task.controlGain.value = 10

    robot.estimator = create_dummy_dcm_estimator(robot)

    # --- Admittance controller
    Kp = [0.0, 0.0, 0.0]
    robot.com_admittance_control = create_com_admittance_controller(
        Kp, dt, robot)

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

    robot.contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF',
                                     robot.OperationalPointsMap['right-ankle'])
    robot.contactRF.feature.frame('desired')
    robot.contactRF.gain.setConstant(100)
    robot.contactRF.keep()
    locals()['contactRF'] = robot.contactRF

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

    robot.sot.push(robot.contactRF.task.name)
    robot.sot.push(robot.contactLF.task.name)
    robot.sot.push(robot.taskCom.task.name)
    robot.device.control.recompute(0)

    # --- TRACER
    robot.tracer = TracerRealTime("zmp_tracer")
    robot.tracer.setBufferSize(80 * (2**20))
    robot.tracer.open('/tmp', 'dg_', '.dat')
    robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))

    addTrace(robot.tracer, robot.dynamic, 'zmp')
    addTrace(robot.tracer, robot.estimator, 'dcm')

    # SIMULATION

    plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN)
    sleep(1.0)
    os.system("rosservice call \start_dynamic_graph")
    sleep(2.0)

    plug(robot.estimator.dcm, robot.com_admittance_control.zmpDes)

    robot.com_admittance_control.setState(robot.dynamic.com.value,
                                          [0.0, 0.0, 0.0])
    robot.com_admittance_control.Kp.value = [10.0, 10.0, 0.0]

    robot.tracer.start()

    sleep(5.0)

    dump_tracer(robot.tracer)

    # --- DISPLAY
    zmp_data = np.loadtxt('/tmp/dg_' + robot.dynamic.name + '-zmp.dat')
    zmpDes_data = np.loadtxt('/tmp/dg_' + robot.estimator.name + '-dcm.dat')

    plt.figure()
    plt.plot(zmp_data[:, 1], 'b-')
    plt.plot(zmpDes_data[:, 1], 'b--')
    plt.plot(zmp_data[:, 2], 'r-')
    plt.plot(zmpDes_data[:, 2], 'r--')
    plt.title('ZMP real vs desired')
    plt.legend(['Real x', 'Desired x', 'Real y', 'Desired y'])

    plt.figure()
    plt.plot(zmp_data[:, 1] - zmpDes_data[:, 1], 'b-')
    plt.plot(zmp_data[:, 2] - zmpDes_data[:, 2], 'r-')
    plt.title('ZMP error')
    plt.legend(['Error on x', 'Error on y'])

    plt.show()
예제 #28
0
task_wrist2 = Task ('wrist2_task')
task_wrist2.controlGain.value = 0.
task_wrist2.add (feature_wrist2.name)


# Create task for the wrist3
I4 =   ((1.,0,0,0.321),
    (0,1.,0,0.109),
    (0,0,1.,0.848),
    (0,0,0,1.),)
feature_wrist = FeaturePosition ('position_wrist', robot.dynamic.wrist_3_joint, robot.dynamic.Jwrist_3_joint, I4)
task_wrist = Task ('wrist_task')
task_wrist.controlGain.value = 1
task_wrist.add (feature_wrist.name)
#Create tracer
tracer = TracerRealTime ('trace')
tracer.setBufferSize(2**20)
tracer.open('/tmp/','dg_','.dat')
# Make sure signals are recomputed even if not used in the control graph
robot.device.after.addSignal('{0}.triger'.format(tracer.name))
addTrace (robot.device, tracer, robot.device.name, "state")
addTrace (robot.device, tracer, feature_wrist._feature.name, "position")
addTrace (robot.device, tracer, feature_wrist._reference.name, "position")


# solver
solver = SOT ('solver')
solver.setSize (dimension)
solver.push (task_waist.name)
solver.push (task_wrist.name)
solver.push (task_elbow.name)
예제 #29
0
task_wrist2.add(feature_wrist2.name)

# Create task for the wrist3
I4 = (
    (1., 0, 0, 0.321),
    (0, 1., 0, 0.109),
    (0, 0, 1., 0.848),
    (0, 0, 0, 1.),
)
feature_wrist = FeaturePosition('position_wrist', robot.dynamic.wrist_3_joint,
                                robot.dynamic.Jwrist_3_joint, I4)
task_wrist = Task('wrist_task')
task_wrist.controlGain.value = 1
task_wrist.add(feature_wrist.name)
#Create tracer
tracer = TracerRealTime('trace')
tracer.setBufferSize(2**20)
tracer.open('/tmp/', 'dg_', '.dat')
# Make sure signals are recomputed even if not used in the control graph
robot.device.after.addSignal('{0}.triger'.format(tracer.name))
addTrace(robot.device, tracer, robot.device.name, "state")
addTrace(robot.device, tracer, feature_wrist._feature.name, "position")
addTrace(robot.device, tracer, feature_wrist._reference.name, "position")

# solver
solver = SOT('solver')
solver.setSize(dimension)
solver.push(task_waist.name)
solver.push(task_wrist.name)
solver.push(task_elbow.name)
solver.push(task_lift.name)
#create_topic(robot.publisher, robot.device_filters.ft_LF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered left wrench
#create_topic(robot.publisher, robot.device_filters.ft_RF_filter, 'x_filtered', robot = robot, data_type='vector') # filtered right wrench

create_topic(robot.publisher,
             robot.ftc,
             'left_foot_force_out',
             robot=robot,
             data_type='vector')  # calibrated left wrench
create_topic(robot.publisher,
             robot.ftc,
             'right_foot_force_out',
             robot=robot,
             data_type='vector')  # calibrated right wrench

# --- TRACER
robot.tracer = TracerRealTime("com_tracer")
robot.tracer.setBufferSize(80 * (2**20))
robot.tracer.open('/tmp', 'dg_', '.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))

addTrace(robot.tracer, robot.wp, 'comDes')  # desired CoM

addTrace(robot.tracer, robot.cdc_estimator, 'c')  # estimated CoM
addTrace(robot.tracer, robot.cdc_estimator, 'dc')  # estimated CoM velocity

addTrace(robot.tracer, robot.com_admittance_control, 'comRef')  # reference CoM
addTrace(robot.tracer, robot.dynamic, 'com')  # resulting SOT CoM

addTrace(robot.tracer, robot.dcm_control, 'dcmDes')  # desired DCM
addTrace(robot.tracer, robot.estimator, 'dcm')  # estimated DCM
예제 #31
0
class AbstractMobileRobot(object):
    OperationalPoints = []
    AdditionalFrames = []   
    name = None
    initPosition = None
    tracer = None
    tracerSize = 2**20
    autoRecomputedSignals = []
    tracedSignals = {
        'dynamic': ["position", "velocity"],
        'device': ['control', 'state']
        }
    # initialize robot    
    def initializeRobot(self):
        if not self.dynamic:
            raise RunTimeError("robots models have to be initialized first")

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

        self.device.set(self.initPosition)
        plug(self.device.state, self.dynamic.position)

        self.dynamic.velocity.value = self.dimension*(0.,)
        self.dynamic.acceleration.value = self.dimension*(0.,)

        self.initializeOpPoints(self.dynamic)

    # create operational points
    def initializeOpPoints(self, model):
        for op in self.OperationalPoints:
            model.createOpPoint(op, op)


    # Tracer methods
    def addTrace(self, entityName, signalName):
        if self.tracer:
            self.autoRecomputedSignals.append(
                '{0}.{1}'.format(entityName, signalName))
            addTrace(self, self.tracer, entityName, signalName)
    def startTracer(self):
        """
        Start the tracer if it does not already been stopped.
        """
        if self.tracer:
            self.tracer.start()
    def stopTracer(self):
        """
        Stop and destroy tracer.
        """
        if self.tracer:
            self.tracer.dump()
            self.tracer.stop()
            self.tracer.close()
            self.tracer.clear()
            for s in self.autoRecomputedSignals:
                self.device.after.rmSignal(s)
            self.tracer = None

    def initializeTracer(self,robotname):

        self.tracer = 0
        self.tracer = TracerRealTime('trace')
        self.tracer.setBufferSize(self.tracerSize)
        self.tracer.open(robotname,'dg_','.dat')
        # Recompute trace.triger at each iteration to enable tracing.
        self.device.after.addSignal('{0}.triger'.format(self.tracer.name))

    def traceDefaultSignals (self):
        # Geometry / operational points
        for s in self.OperationalPoints + self.tracedSignals['dynamic']:
            self.addTrace(self.dynamic.name, s)
        # Device
        for s in self.tracedSignals['device']:
            self.addTrace(self.device.name, s)
        if type(self.device) != RobotSimu:
            self.addTrace(self.device.name, 'robotState')

    # const and deconst
    def __init__(self, name, tracer = None):
        self.name = name
        # Initialize tracer if necessary.
        if tracer:
            self.tracer = tracer

    def __del__(self):
        if self.tracer:
            self.stopTracer()