def init_appli(robot): taskRH = MetaTaskKine6d('rh', robot.dynamic, 'rh', robot.OperationalPointsMap['right-wrist']) handMgrip = eye(4) handMgrip[0:3, 3] = (0.1, 0, 0) taskRH.opmodif = matrixToTuple(handMgrip) taskRH.feature.frame('desired') # --- STATIC COM (if not walking) taskCom = MetaTaskKineCom(robot.dynamic) robot.dynamic.com.recompute(0) taskCom.featureDes.errorIN.value = robot.dynamic.com.value taskCom.task.controlGain.value = 10 # --- CONTACTS contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF', robot.OperationalPointsMap['left-ankle']) contactLF.feature.frame('desired') contactLF.gain.setConstant(10) contactLF.keep() locals()['contactLF'] = contactLF contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) contactRF.feature.frame('desired') contactRF.gain.setConstant(10) contactRF.keep() locals()['contactRF'] = contactRF from dynamic_graph import plug from dynamic_graph.sot.core.sot import SOT sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) plug(sot.control, robot.device.control) from dynamic_graph.ros import RosPublish ros_publish_state = RosPublish("ros_publish_state") ros_publish_state.add("vector", "state", "/sot_control/state") from dynamic_graph import plug plug(robot.device.state, ros_publish_state.state) robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100) sot.push(contactRF.task.name) sot.push(contactLF.task.name) sot.push(taskCom.task.name) robot.device.control.recompute(0)
taskRH.opmodif = matrixToTuple(handMgrip) taskRH.feature.frame('desired') projection = HolonomicProjection("projection") projection.setSize(robot.dynamic.getDimension()) projection.setLeftWheel(6) projection.setRightWheel(7) # The wheel separation could be obtained with pinocchio. # See pmb2_description/urdf/base.urdf.xacro projection.setWheelRadius(0.0985) projection.setWheelSeparation(0.4044) plug(robot.dynamic.mobilebase, projection.basePose) sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) plug(projection.projection, sot.proj0) plug(sot.control, robot.device.control) ros_publish_state = RosPublish("ros_publish_state") ros_publish_state.add("vector", "state", "/sot_control/state") plug(robot.device.state, ros_publish_state.state) robot.device.after.addDownsampledSignal("ros_publish_state.trigger", 100) target = (0.5, -0.2, 1.0) gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9)) sot.push(taskRH.task.name) robot.device.control.recompute(0)
contactRF = MetaTaskKine6d('contactRF', robot.dynamic, 'RF', robot.OperationalPointsMap['right-ankle']) contactRF.feature.frame('desired') contactRF.gain.setConstant(10) contactRF.keep() targetRH = (0.5, -0.2, 1.0) targetLH = (0.5, 0.2, 1.0) # addRobotViewer(robot, small=False) # robot.viewer.updateElementConfig('zmp',target+(0,0,0)) gotoNd(taskRH, targetRH, '111', (4.9, 0.9, 0.01, 0.9)) gotoNd(taskLH, targetLH, '111', (4.9, 0.9, 0.01, 0.9)) sot.push(contactRF.task.name) sot.push(contactLF.task.name) sot.push(taskCom.task.name) sot.push(taskLH.task.name) sot.push(taskRH.task.name) toe = TOE("test_entity_toe") id4 = ((1.0, 0.0, 0.0, 0.0), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, 1.0, 0.0), (0.0, 0.0, 0.0, 1.0)) epsilon = 0.1 ref_offset = 0.1 tau_offset = np.asarray((ref_offset, ) * njoints).squeeze() toe.init(urdfPath, id4, epsilon, OperationalPointsMap['waist'], OperationalPointsMap['chest']) plug(robot.device.state, toe.base6d_encoders) toe.gyroscope.value = (0., 0., 0.)
taskTwofeet.signal('controlGain').value = 0. # --- CONTACT CONSTRAINT legs = Constraint('legs') legs.addJacobian('dyn.Jlleg') # --- SOT sot = SOT('sot') sot.signal('damping').value = 1e-6 sot.addConstraint('legs') sot.setNumberDofs(dimension) # plug dyn.inertia sot.weight # sot.push taskTwofeet # sot.push taskLegs sot.push('task') sot.push('taskCom') # sot.push taskChest # sot.gradient taskJl featureComDes.signal('errorIN').value = (0.004,-0.09,.6) zeroCom = VectorConstant('zeroCom') zeroCom.set(dimension*(0.,)) dyn.createOpPoint('Waist', 'waist') OpenHRP = RobotSimu('OpenHRP') OpenHRP.set((0.,0.,0.,0.,0.,0.,0.,0.,-1.04720,2.09440,-1.04720,0.,0.,0.,-1.04720,2.09440,-1.04720,0.,0.0000,0.,-0.,-0.,0.17453,-0.17453,-0.17453,-0.87266,0.,-0.,0.1,0.,0.17453,-0.17453,-0.17453,-0.87266,0.,-0.,0.1,0.))
taskTwofeet.signal('controlGain').value = 0. # --- CONTACT CONSTRAINT legs = Constraint('legs') legs.addJacobian('dyn.Jlleg') # --- SOT sot = SOT('sot') sot.signal('damping').value = 1e-6 sot.addConstraint('legs') sot.setNumberDofs(dimension) # plug dyn.inertia sot.weight # sot.push taskTwofeet # sot.push taskLegs sot.push('task') # sot.push taskCom # sot.push taskChest # sot.gradient taskJl featureComDes.signal('errorIN').value = (0.004, -0.09, .6) zeroCom = VectorConstant('zeroCom') zeroCom.set(dimension * (0., )) dyn.createOpPoint('Waist', 'waist') OpenHRP = RobotSimu('OpenHRP') OpenHRP.set((0., 0., 0., 0., 0., 0., 0., 0., -1.04720, 2.09440, -1.04720, 0., 0., 0., -1.04720, 2.09440, -1.04720, 0., 0.0000, 0., -0., -0., 0.17453, -0.17453, -0.17453, -0.87266, 0., -0., 0.1, 0., 0.17453,
plug(seqplay.posture, taskPosture.featureDes.errorIN) getPostureValue = Selec_of_vector("current_posture") getPostureValue.selec(6, robotDim) plug(robot.dynamic.position, getPostureValue.sin) plug(getPostureValue.sout, taskPosture.feature.errorIN) plug(getPostureValue.sout, seqplay.currentPosture) setGain(taskPosture.gain, (4.9, 0.9, 0.01, 0.9)) plug(taskPosture.gain.gain, taskPosture.controlGain) plug(taskPosture.error, taskPosture.gain.error) # START SEQUENCE PLAYER seqplay.start() taskPosture.featureDes.errorIN.recompute(0) # PUSH TO SOLVER sot.push(taskPosture.name) # ------------------------------------------------------------------------------- # ----- MAIN LOOP --------------------------------------------------------------- # ------------------------------------------------------------------------------- def runner(n): for i in range(n): robot.device.increment(dt) pinocchioRobot.display(fromSotToPinocchio(robot.device.state.value)) runner(3000)
taskTwofeet.signal('controlGain').value = 0. # --- CONTACT CONSTRAINT legs = Constraint('legs') legs.addJacobian('dyn.Jlleg') # --- SOT sot = SOT('sot') sot.signal('damping').value = 1e-6 sot.addConstraint('legs') sot.setNumberDofs(dimension) # plug dyn.inertia sot.weight # sot.push taskTwofeet # sot.push taskLegs sot.push('task') # sot.push taskCom # sot.push taskChest # sot.gradient taskJl featureComDes.signal('errorIN').value = (0.004,-0.09,.6) zeroCom = VectorConstant('zeroCom') zeroCom.set(dimension*(0.,)) dyn.createOpPoint('Waist', 'waist') OpenHRP = RobotSimu('OpenHRP') OpenHRP.set((0.,0.,0.,0.,0.,0.,0.,0.,-1.04720,2.09440,-1.04720,0.,0.,0.,-1.04720,2.09440,-1.04720,0.,0.0000,0.,-0.,-0.,0.17453,-0.17453,-0.17453,-0.87266,0.,-0.,0.1,0.,0.17453,-0.17453,-0.17453,-0.87266,0.,-0.,0.1,0.))
# define contactLF and contactRF for name, joint in [['LF', robot.OperationalPointsMap['left-ankle']], ['RF', robot.OperationalPointsMap['right-ankle']]]: contact = MetaTaskKine6d('contact' + name, robot.dynamic, name, joint) contact.feature.frame('desired') contact.gain.setConstant(10) contact.keep() locals()['contact' + name] = contact target = (0.5, -0.2, 1.0) # addRobotViewer(robot, small=False) # robot.viewer.updateElementConfig('zmp',target+(0,0,0)) gotoNd(taskRH, target, '111', (4.9, 0.9, 0.01, 0.9)) sot.push(contactRF.task.name) # noqa TODO sot.push(contactLF.task.name) # noqa TODO sot.push(taskCom.task.name) sot.push(taskRH.task.name) # ------------------------------------------------------------------------------- # ----- MAIN LOOP --------------------------------------------------------------- # ------------------------------------------------------------------------------- def runner(n): for i in range(n): robot.device.increment(dt) pinocchioRobot.display(fromSotToPinocchio(robot.device.state.value))