def __init__(self, robot, taskname="com-stabilized"): from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_base_flex_estimator_imu_force_encoders import ( HRP2ModelBaseFlexEstimatorIMUForceEncoders, ) from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_base_flex_estimator_imu_force import ( HRP2ModelBaseFlexEstimatorIMUForce, ) HRP2LQRTwoDofCoupledStabilizer.__init__(self, taskname) robot.dynamic.com.recompute(0) robot.dynamic.Jcom.recompute(0) robot.dynamic.waist.recompute(0) robot.dynamic.Jwaist.recompute(0) robot.dynamic.inertia.recompute(0) # Reconstruction of the control state # DCoM self.DCom = Multiply_matrix_vector("DCom") plug(robot.dynamic.Jcom, self.DCom.sin1) plug(robot.device.velocity, self.DCom.sin2) # DWaist self.DWaist = Multiply_matrix_vector("DWaist") plug(robot.dynamic.Jwaist, self.DWaist.sin1) plug(robot.device.velocity, self.DWaist.sin2) # Estimator of the flexibility state self.estimatorEnc = HRP2ModelBaseFlexEstimatorIMUForceEncoders(robot, taskname + "EstimatorEncoders") self.estimator = HRP2ModelBaseFlexEstimatorIMUForce(robot, taskname + "Estimator") plug(self.estimatorEnc.interface.supportContactsNbr, self.nbSupport) plug(self.estimatorEnc.flexPosition, self.tflex) plug(self.estimatorEnc.flexVelocity, self.dtflex) # Control state plug(robot.dynamic.com, self.com) plug(robot.dynamic.waist, self.waistHomo) plug(self.estimatorEnc.flexThetaU, self.flexOriVect) plug(self.DCom.sout, self.comDot) plug(self.DWaist.sout, self.waistAngVel) plug(self.estimatorEnc.flexOmega, self.flexAngVelVect) # Jacobians plug(robot.dynamic.Jcom, self.Jcom) plug(robot.dynamic.Jchest, self.Jchest) plug(robot.dynamic.Jwaist, self.Jwaist) # Inertia plug(robot.dynamic.inertia, self.inertia) plug(robot.dynamic.angularmomentum, self.angularmomentum)
def __init__(self,robot,taskname = 'com-stabilized'): from dynamic_graph.sot.application.state_observation.initializations.hrp2_model_base_flex_estimator_imu_force import HRP2ModelBaseFlexEstimatorIMUForce HRP2LQRTwoDofCoupledStabilizer.__init__(self,taskname) robot.device.state.value=robot.halfSitting robot.device.velocity.value=(0.,)*36 robot.device.forceLLEG.value=(1.8349814919184242, -7.4412430930486302, 256.06853454222494, -0.035428813912447302, 2.0798475785647286, 0.14169701504511384) robot.device.forceRLEG.value=(2.4303733340459406, 11.156361786170869, 285.59013529212666, -0.69957871247984049, 2.0516111892090887, -0.22872430884228223) robot.device.accelerometer.value=(0.12527866711822, 0.089756740219665537, 9.8059788372472152) robot.device.gyrometer.value=(-0.0029326257213877862, 0.007655425240526083, -8.001571126249214e-05) robot.device.forceLARM.value=(-2.04071, 3.25524, -5.89116, 0.0814646, 0.0779619, 0.0190317) robot.device.forceRARM.value=(2.07224, -9.57906, 0.69111, -0.415802, 0.289026, 0.00621748) def recomputeDynamic(i): robot.dynamic.chest.recompute(i) robot.dynamic.com.recompute(i) robot.dynamic.Jcom.recompute(i) robot.dynamic.angularmomentum.recompute(i) robot.dynamic.inertia.recompute(i) robot.dynamic.waist.recompute(i) recomputeDynamic(0) # DCoM self.DCom = Multiply_matrix_vector('DCom') # Com velocity: self.DCom.sout plug(robot.dynamic.Jcom,self.DCom.sin1) plug(robot.device.velocity,self.DCom.sin2) # DWaist self.DWaist = Multiply_matrix_vector('DWaist') # Waist angulat velocity: self.DWaist.sout plug(robot.dynamic.Jwaist,self.DWaist.sin1) plug(robot.device.velocity,self.DWaist.sin2) # Estimator of the flexibility state self.estimator = HRP2ModelBaseFlexEstimatorIMUForce (robot, taskname+"Estimator") plug (self.estimator.interface.modeledContactsNbr,self.nbSupport) plug(self.estimator.flexPosition, self.tflex) plug(self.estimator.flexVelocity, self.dtflex) self.estimator.state.recompute(2) recomputeDynamic(2) # Control state plug(robot.dynamic.com, self.com) plug(robot.dynamic.waist,self.waistHomo) plug(self.estimator.flexThetaU, self.flexOriVect ) plug(self.DCom.sout,self.comDot) plug(self.DWaist.sout,self.waistAngVel) plug(self.estimator.flexOmega, self.flexAngVelVect ) # CoM bias plug(self.estimator.comBias,self.comBias) # Jacobians plug ( robot.dynamic.Jcom , self.Jcom ) plug ( robot.dynamic.Jwaist, self.Jwaist) plug ( robot.dynamic.Jchest, self.Jchest) # Inertial values plug ( robot.dynamic.inertia, self.inertia) plug(robot.dynamic.angularmomentum,self.angularmomentum)