def initPostureTask(robot): # --- TASK POSTURE -------------------------------------------------- # set a default position for the joints. robot.features['featurePosition'] = FeaturePosture('featurePosition') plug(robot.device.state, robot.features['featurePosition'].state) robotDim = len(robot.dynamic.velocity.value) robot.features['featurePosition'].posture.value = robot.halfSitting # Remove the dofs of the feet. postureTaskDofs = [True] * (len(robot.dynamic.position.value) - 6) jla = robot.dynamic.signal('J' + robot.OperationalPointsMap['left-ankle']).value postureTaskDofs = removeDofUsed(jla, postureTaskDofs) jra = robot.dynamic.signal('J' + robot.OperationalPointsMap['right-ankle']).value postureTaskDofs = removeDofUsed(jra, postureTaskDofs) for dof, isEnabled in enumerate(postureTaskDofs): robot.features['featurePosition'].selectDof(dof + 6, isEnabled) robot.tasks['robot_task_position'] = Task('robot_task_position') robot.tasks['robot_task_position'].add('featurePosition') gainPosition = GainAdaptive('gainPosition') gainPosition.set(0.1, 0.1, 125e3) gainPosition.gain.value = 5 plug(robot.tasks['robot_task_position'].error, gainPosition.error) plug(gainPosition.gain, robot.tasks['robot_task_position'].controlGain)
def __init__(self, sotrobot, gripper, position): from dynamic_graph.sot.core import Task, GainAdaptive super(EEPosture, self).__init__() self.gripper = gripper self.jointNames = gripper.joints pinmodel = sotrobot.dynamic.model n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str( list(position)) self.tp = Task('task' + n) self.tp.dyn = sotrobot.dynamic self.tp.feature = FeaturePosture('feature_' + n) plug(sotrobot.dynamic.position, self.tp.feature.state) q = list(sotrobot.dynamic.position.value) # Define the reference and the selected DoF rank = 0 ranks = [] for name in self.jointNames: idJ = pinmodel.getJointId(name) assert idJ < pinmodel.njoints joint = pinmodel.joints[idJ] idx_v = joint.idx_v nv = joint.nv ranks += range(idx_v, idx_v + nv) q[idx_v:idx_v + nv] = position[rank:rank + nv] rank += nv assert rank == len(position) self.tp.feature.posture.value = q for i in ranks: self.tp.feature.selectDof(i, True) self.tp.gain = GainAdaptive("gain_" + n) self.tp.add(self.tp.feature.name) # Set the gain of the posture task setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9)) plug(self.tp.gain.gain, self.tp.controlGain) plug(self.tp.error, self.tp.gain.error) if len(self.jointNames) > 0: self.tasks = [self.tp]
def __init__ (self, sotrobot, gripper, position): from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive, Selec_of_vector from dynamic_graph.sot.core.matrix_util import matrixToTuple from dynamic_graph import plug from numpy import identity, hstack, zeros super(EEPosture, self).__init__() self.gripper = gripper # Get joint position in posture pinmodel = sotrobot.dynamic.model idJ = pinmodel.getJointId(gripper.sotjoint) assert idJ < pinmodel.njoints joint = sotrobot.dynamic.model.joints[idJ] assert joint.nq == len(position) idx_q = joint.idx_q + 1 idx_v = joint.idx_v + 1 n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(position) self.tp = Task ('task' + n) self.tp.dyn = sotrobot.dynamic self.tp.feature = FeaturePosture ('feature_' + n) plug(sotrobot.dynamic.position, self.tp.feature.state) q = list(sotrobot.dynamic.position.value) q[idx_v:idx_v + joint.nv] = position self.tp.feature.posture.value = q self.tp.gain = GainAdaptive("gain_"+n) robotDim = sotrobot.dynamic.getDimension() # for i in range (6, robotDim): # self.tp.feature.selectDof (i, False) # print idx_q, idx_v for i in range(joint.nv): self.tp.feature.selectDof (idx_v + i, True) self.tp.add(self.tp.feature.name) # Set the gain of the posture task setGain(self.tp.gain,(4.9,0.9,0.01,0.9)) plug(self.tp.gain.gain, self.tp.controlGain) plug(self.tp.error, self.tp.gain.error) self.tasks = [ self.tp ]
def __init__(self, name, sotrobot): super(Posture, self).__init__() from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive from dynamic_graph.sot.core.matrix_util import matrixToTuple from numpy import identity n = Posture.sep + name self.tp = Task('task' + n) self.tp.dyn = sotrobot.dynamic self.tp.feature = FeaturePosture('feature_' + n) q = list(sotrobot.dynamic.position.value) self.tp.feature.state.value = q self.tp.feature.posture.value = q robotDim = sotrobot.dynamic.getDimension() for i in range(6, robotDim): self.tp.feature.selectDof(i, True) self.tp.gain = GainAdaptive("gain_" + n) self.tp.add(self.tp.feature.name) # Connects the dynamics to the current feature of the posture task plug(sotrobot.dynamic.position, self.tp.feature.state) self.tp.setWithDerivative(True) # Set the gain of the posture task setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9)) plug(self.tp.gain.gain, self.tp.controlGain) plug(self.tp.error, self.tp.gain.error) self.tasks = [self.tp] self.topics = { name: { "type": "vector", "topic": "/hpp/target/position", "signalGetters": [self._signalPositionRef] }, "vel_" + name: { "type": "vector", "topic": "/hpp/target/velocity", "signalGetters": [self._signalVelocityRef] }, }
def initPostureTask(robot): # --- TASK POSTURE -------------------------------------------------- # set a default position for the joints. robot.features['featurePosition'] = FeaturePosture('featurePosition') plug(robot.device.state, robot.features['featurePosition'].state) robotDim = len(robot.dynamic.velocity.value) robot.features['featurePosition'].posture.value = robot.halfSitting if robot.device.name == 'HRP2LAAS' or \ robot.device.name == 'HRP2JRL': postureTaskDofs = [ False,False,False,False,False,False, \ False,False,False,False,False,False, \ True,True,True,True, \ True,True,True,True,True,True,True, \ True,True,True,True,True,True,True ] elif robot.device.name == 'HRP4LIRMM': # Right Leg, Left leg, chest, right arm, left arm postureTaskDofs = [False] * 6 + [False] * 6 + [True] * 4 + [ True ] * 9 + [True] * 9 elif robot.device.name == 'ROMEO': # chest, left/right arms, left/right legs postureTaskDofs = [True] * 5 + [True] * 7 + [True] * 7 + [ False ] * 7 + [False] * 7 else: print "/!\\ walking.py: The robot " + robot.device.name + " is unknown." print " Default posture task froze all the dofs" postureTaskDofs = [True] * (robot.dimension - 6) for dof, isEnabled in enumerate(postureTaskDofs): robot.features['featurePosition'].selectDof(dof + 6, isEnabled) robot.tasks['robot_task_position'] = Task('robot_task_position') robot.tasks['robot_task_position'].add('featurePosition') # featurePosition.selec.value = toFlags((6,24)) gainPosition = GainAdaptive('gainPosition') gainPosition.set(0.1, 0.1, 125e3) gainPosition.gain.value = 5 plug(robot.tasks['robot_task_position'].error, gainPosition.error) plug(gainPosition.gain, robot.tasks['robot_task_position'].controlGain)
def __init__(self, sotrobot, gripper, name_suffix): from dynamic_graph.sot.core import Task, GainAdaptive super(EndEffector, self).__init__() self.gripper = gripper self.jointNames = gripper.joints self.robot = sotrobot pinmodel = sotrobot.dynamic.model self.name = Posture.sep.join(["ee", gripper.name, name_suffix]) self.tp = Task('task' + self.name) self.tp.dyn = sotrobot.dynamic self.tp.feature = FeaturePosture('feature_' + self.name) plug(sotrobot.dynamic.position, self.tp.feature.state) # Select the dofs self.tp.feature.posture.value = sotrobot.dynamic.position.value # Define the reference and the selected DoF self.jointRanks = [] for name in self.jointNames: idJ = pinmodel.getJointId(name) assert idJ < pinmodel.njoints joint = pinmodel.joints[idJ] idx_v = joint.idx_v nv = joint.nv self.jointRanks.append((idx_v, nv)) for idx_v, nv in self.jointRanks: for i in range(idx_v, idx_v + nv): self.tp.feature.selectDof(i, True) self.tp.gain = GainAdaptive("gain_" + self.name) self.tp.add(self.tp.feature.name) # Set the gain of the posture task setGain(self.tp.gain, (4.9, 0.9, 0.01, 0.9)) plug(self.tp.gain.gain, self.tp.controlGain) plug(self.tp.error, self.tp.gain.error) if len(self.jointNames) > 0: self.tasks = [self.tp]
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') robot.cm.addEmergencyStopSIN('distribute') # -------------------------- 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 for i in range(18, robotDim): robot.taskUpperBody.feature.selectDof(i, 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
controller.init(timeStep, 1) controller.setPosition([robot.device.state.value[LeftRollJoint + 6]]) robot.leftRollAnkleController = controller robot.leftAnkleRollTask = MetaTaskKineJoint(robot.dynamic, LeftRollJoint) robot.leftAnkleRollTask.task.controlGain.value = 0 robot.leftAnkleRollTask.task.setWithDerivative(True) plug(robot.leftRollAnkleController.qRef, robot.leftAnkleRollTask.featureDes.errorIN) plug(robot.leftRollAnkleController.dqRef, robot.leftAnkleRollTask.featureDes.errordotIN) # -------------------------- SOT CONTROL -------------------------- # --- Posture robot.taskPosture = Task('taskPosture') robot.taskPosture.feature = FeaturePosture('featurePosture') q = list(robot.dynamic.position.value) robot.taskPosture.feature.state.value = q robot.taskPosture.feature.posture.value = q for i in range(18, 38): robot.taskPosture.feature.selectDof(i, True) robot.taskPosture.controlGain.value = 100.0 robot.taskPosture.add(robot.taskPosture.feature.name) plug(robot.dynamic.position, robot.taskPosture.feature.state) # --- CONTACTS # define contactLF and contactRF robot.contactLF = MetaTaskKine6d('contactLF', robot.dynamic, 'LF',
# flake8: noqa from dynamic_graph import plug from dynamic_graph.sot.core import SOT, FeatureGeneric, FeaturePosture, GainAdaptive, Task from dynamic_graph.sot.core.matrix_util import matrixToTuple from dynamic_graph.sot.core.meta_tasks import setGain from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd from dynamic_graph.sot.tiago.diff_drive_controller import HolonomicProjection n = "posture" tp = Task('task' + n) tp.dyn = robot.dynamic tp.feature = FeaturePosture('feature_' + n) q = list(robot.device.robotState.value) tp.feature.state.value = q tp.feature.posture.value = q robotDim = robot.dynamic.getDimension() assert robotDim == len(q) with_wheels = robotDim == 19 for i in range(8 if with_wheels else 6, robotDim): tp.feature.selectDof(i, True) tp.gain = GainAdaptive("gain_" + n) tp.add(tp.feature.name) # Connects the dynamics to the current feature of the posture task plug(robot.dynamic.position, tp.feature.state)