def main(robot, conf=None): if (conf is None): conf = get_default_conf() robot.timeStep = robot.device.getTimeStep() dt = robot.timeStep NJ = robot.dimension - 7 robot.comTrajGen = create_com_trajectory_generator(dt, robot) # --- 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 # --- 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 # --- SOT 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.taskCom.task.name) robot.sot.push(robot.contactLF.task.name) robot.device.control.recompute(0) # --- ENTITIES robot.param_server = create_parameter_server(conf.param_server, dt) robot.example = create_example()
fz = m * g force = [0.0, 0.0, fz] tau = np.cross(comRel, np.matrix(force).T, axis=0) wrench = force + tau.flatten().tolist() print("desired wrench: %s" % str(wrench)) # --- Desired CoM, DCM and ZMP comDes = tuple(comRel.flatten().tolist()[0]) dcmDes = comDes zmpDes = comDes[:2] + (0.0, ) # --- Parameter server --- print("--- Parameter server ---") param_server = create_parameter_server(param_server_conf, dt) # --- DCM controller Kp_dcm = [0.0, 0.0, 0.0] Ki_dcm = [0.0, 0.0, 0.0] 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 = m dcm_controller.omega.value = omega dcm_controller.com.value = comDes