pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0") pg.parseCmd(":comheight 0.814") pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa") plug(robot.dynamic.position,pg.position) plug(robot.dynamic.com,pg.com) pg.motorcontrol.value = robot.dynamic.getDimension()*(0,) pg.zmppreviouscontroller.value = (0,0,0) pg.initState() geom = RosSotRobotModel('geom') geom.setNamespace('sot_controller') geom.loadFromParameterServer() geom.createOpPoint('rf','right-ankle') geom.createOpPoint('lf','left-ankle') plug(robot.dynamic.position,geom.position) geom.ffposition.value = 6*(0,) geom.velocity.value = robot.dynamic.getDimension()*(0,) geom.acceleration.value = robot.dynamic.getDimension()*(0,) comRef = Selector('comRef',['vector','ref',robot.dynamic.com,pg.comref]) plug(pg.inprocess,comRef.selec) selecSupportFoot = Selector('selecSupportFoot',['matrixHomo','pg_H_sf',pg.rightfootref,pg.leftfootref],['matrixHomo','wa_H_sf',geom.rf,geom.lf]) plug(pg.SupportFoot,selecSupportFoot.selec) sf_H_wa = Inverse_of_matrixHomo('sf_H_wa') plug(selecSupportFoot.wa_H_sf,sf_H_wa.sin) pg_H_wa = Multiply_of_matrixHomo('pg_H_wa')
pg.parseCmd(":UpperBodyMotionParameters 0.0 -0.5 0.0") pg.parseCmd(":comheight 0.814") pg.parseCmd(":SetAlgoForZmpTrajectory Morisawa") plug(robot.dynamic.position, pg.position) plug(robot.dynamic.com, pg.com) pg.motorcontrol.value = robot.dynamic.getDimension() * (0, ) pg.zmppreviouscontroller.value = (0, 0, 0) pg.initState() geom = RosSotRobotModel('geom') geom.setNamespace('sot_controller') geom.loadFromParameterServer() geom.createOpPoint('rf', 'right-ankle') geom.createOpPoint('lf', 'left-ankle') plug(robot.dynamic.position, geom.position) geom.ffposition.value = 6 * (0, ) geom.velocity.value = robot.dynamic.getDimension() * (0, ) geom.acceleration.value = robot.dynamic.getDimension() * (0, ) comRef = Selector('comRef', ['vector', 'ref', robot.dynamic.com, pg.comref]) plug(pg.inprocess, comRef.selec) selecSupportFoot = Selector( 'selecSupportFoot', ['matrixHomo', 'pg_H_sf', pg.rightfootref, pg.leftfootref], ['matrixHomo', 'wa_H_sf', geom.rf, geom.lf]) plug(pg.SupportFoot, selecSupportFoot.selec) sf_H_wa = Inverse_of_matrixHomo('sf_H_wa')