def makeInitialSot(self): # Create the initial sot (keep) sot = SOT('sot_keep') sot.setSize(self.sotrobot.dynamic.getDimension()) self.keep_posture = Posture("posture_keep", self.sotrobot) self.keep_posture.tp.setWithDerivative(False) # TODO : I do agree that this is a dirty hack. # The COM of the robot in the HPP frame is at those coordinates (approx.). # But the keep_posture task is « internally » (there is no actuator able to directly move the COM, # but the controller in the task is computing controls anyway, and integrating them) # moving the computed COM to its reference value which is (0, 0, 0). # So, when we send the goal coordinates of the feet from HPP to the SoT, there is an offset of 0,74m # between the goal and the current position of the feet. This was the cause of the strange feet # movements at the beginning of the demo. # Maybe we can get the COM position and orientation from HPP at the beginning of the trajectory # to initialize self.sotrobot.dynamic.position.value # self.keep_posture._signalPositionRef().value = tuple([-0.74, 0.0, 1.0, 0.0, 0.0, 0.0] + list(self.sotrobot.dynamic.position.value)[6:]) # The above TODO must be fixed in users script by providing the # right initial pose using robot.device.set (configuration) before starting # dynamic graph. self.keep_posture._signalPositionRef( ).value = self.sotrobot.dynamic.position.value self.keep_posture.pushTo(sot) self.sots[""] = sot
def makeGrasps (self, transitions): """ @param transition: a list of dictionaries containing the following keys: - "id", "name": the id and name of the transition in hpp. - "manifold": a tuple of (gripper_index, handle_index) defining the submanifold into which the transition takes place. - "grasp" (optional) : (gripper_index, handle_index) corresponding to the added/removed grasp. - "forward" (required if "grasp") : True if added, False if removed. - "step" (required if "grasp") : integer [ 0, 5 ]. """ self.grasps = dict() self.sots = dict() self.postActions = dict() self.preActions = dict() self.transitions = transitions print "Transitions:\n" print transitions print "\n" for t in transitions: # Create SOT solver # sot = SOT ('sot_' + str(t['id']) + '-' + t['name']) sot = SOT ('sot_' + str(t['id'])) sot.setSize(self.sotrobot.dynamic.getDimension()) # Push high priority tasks (Equilibrium for instance) self.hpTasks.pushTo(sot) # Create (or get) the tasks M = self._manifold(t["manifold"]) if t.has_key("grasp"): grasp = self._manifold(t["manifold"] + (t["grasp"],)) forward = bool(t["forward"]) step = int(t["step"]) assert step >= 0 and step <= 5, "'step' must be an integer within [0, 5]" # TODO Events should be set up here for each step. # For instance, adding some events based on force feedback to say # when the object is grasped. if forward: if step == 1: M.pushTo(sot) elif step == 0 or step == 2: grasp.pushTo(sot) else: grasp.pushTo(sot) else: if step == 1: M.pushTo(sot) elif step == 0 or step == 2: grasp.pushTo(sot) else: grasp.pushTo(sot) else: M.pushTo(sot) # Put low priority tasks self.lpTasks.pushTo(sot) self.sots[t['id']] = sot
def makeLoopTransition(self, state): n = self._loopTransitionName(state.grasps) sot = SOT('sot_' + n) sot.setSize(self.sotrobot.dynamic.getDimension()) self.hpTasks.pushTo(sot) state.manifold.pushTo(sot) self.lpTasks.pushTo(sot) self.sots[n] = sot
def _newSoT(self, name): sot = SOT(name) sot.setSize(self.sotrobot.dynamic.getDimension()) sot.damping.value = 0.001 if self.addTimerToSotControl: from .tools import insertTimerOnOutput self.timers[name] = insertTimerOnOutput(sot.control, "vector") self.SoTtracer.add(self.timers[name].name + ".timer", str(len(self.timerIds)) + ".timer") self.timerIds.append(name) return sot
def __init__(self, name, dimension, damping=None, timer=None): from dynamic_graph.sot.core import SOT sot = SOT(name) sot.setSize(dimension) if damping is not None: sot.damping.value = damping self.sot = sot self.tasks = [] if timer: from .tools import insertTimerOnOutput self.timer = insertTimerOnOutput(sot.control, "vector") else: self.timer = None
def main(robot): robot.timeStep = robot.device.getTimeStep() dt = robot.timeStep robot.traj_gen = create_config_trajectory_generator(dt, robot) JOINT = 31 QJOINT = JOINT + 6 # --- Joint robot.taskJoint = MetaTaskKineConfig(robot.dynamic, [QJOINT]) # robot.dynamic.com.recompute(0) robot.taskJoint.featureDes.errorIN.value = N_CONFIG * [0.0] robot.taskJoint.task.controlGain.value = 100 # --- 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 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.contactLF.task.name) robot.sot.push(robot.taskJoint.task.name) robot.device.control.recompute(0) plug(robot.traj_gen.x, robot.taskJoint.featureDes.errorIN) sleep(1.0) os.system('rosservice call /start_dynamic_graph') sleep(1.0) robot.traj_gen.move(QJOINT, -1.0, 1.0) sleep(1.1) robot.traj_gen.startSinusoid(QJOINT, 1.0, 8.0) sleep(10.0) robot.traj_gen.stop(QJOINT)
def __init__(self, name, dimension, damping=None, timer=False): from dynamic_graph.entity import VerbosityLevel from dynamic_graph.sot.core import SOT sot = SOT(name) sot.setSize(dimension) sot.setMaxControlIncrementSquaredNorm(10.) sot.setLoggerVerbosityLevel(VerbosityLevel.VERBOSITY_ALL) if damping is not None: sot.damping.value = damping self.sot = sot self.tasks = [] if timer: from .tools import insertTimerOnOutput self.timer = insertTimerOnOutput(sot.control, "vector") else: self.timer = None
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()
plug(robot.com_admittance_control.dcomRef, robot.taskCom.featureDes.errordotIN) robot.taskCom.task.controlGain.value = 100. robot.taskCom.task.setWithDerivative(True) robot.taskCom.feature.selec.value = '011' # --- Waist robot.keepWaist = MetaTaskKine6d('keepWaist', robot.dynamic, 'WT', robot.OperationalPointsMap['waist']) robot.keepWaist.feature.frame('desired') robot.keepWaist.gain.setConstant(300) plug(robot.wp.waistDes, robot.keepWaist.featureDes.position) robot.keepWaist.feature.selec.value = '111000' locals()['keepWaist'] = robot.keepWaist # --- SOT solver robot.sot = SOT('sot') robot.sot.setSize(robot.dynamic.getDimension()) # --- Plug SOT control to device through control manager plug(robot.sot.control, robot.cm.ctrl_sot_input) plug(robot.cm.u_safe, robot.device.control) robot.sot.push(robot.taskUpperBody.name) robot.sot.push(robot.contactRF.task.name) robot.sot.push(robot.contactLF.task.name) robot.sot.push(robot.taskComH.task.name) robot.sot.push(robot.taskCom.task.name) robot.sot.push(robot.keepWaist.task.name) # robot.sot.push(robot.taskPos.name) # robot.device.control.recompute(0) # this crashes as it employs joint sensors which are not ready yet
"/home/ostasse/devel-src/robotpkg-test3/install/share/pyrene-motions/identification/OEM_arms_60s_500Hz" ) aSimpleSeqPlay.setTimeToStart(10.0) # Connects the sequence player to the posture task from dynamic_graph.sot.core import Selec_of_vector from dynamic_graph import plug plug(aSimpleSeqPlay.posture, taskPosture.featureDes.errorIN) # Connects the dynamics to the current feature of the posture task 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, aSimpleSeqPlay.currentPosture) # Set the gain of the posture task setGain(taskPosture.gain, (4.9, 0.9, 0.01, 0.9)) plug(taskPosture.gain.gain, taskPosture.controlGain) plug(taskPosture.error, taskPosture.gain.error) # Create the solver from dynamic_graph.sot.core import SOT sot = SOT('sot') sot.setSize(robot.dynamic.getDimension()) plug(sot.control, robot.device.control) taskPosture.featureDes.errorIN.recompute(0) # Push the posture task in the solver
def main(robot): robot.timeStep = robot.device.getTimeStep() dt = robot.timeStep 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) # --- ESTIMATION robot.param_server = create_parameter_server(param_server_conf, dt) # robot.imu_offset_compensation = create_imu_offset_compensation(robot, dt) robot.device_filters = create_device_filters(robot, dt) robot.imu_filters = create_imu_filters(robot, dt) robot.base_estimator = create_base_estimator(robot, dt, conf) robot.be_filters = create_be_filters(robot, dt) # --- TRACERS outputs = ['robotState'] robot.device_tracer = create_tracer(robot, robot.device, 'device_tracer', outputs) outputs = ['q'] robot.estimator_tracer = create_tracer(robot, robot.base_estimator, 'estimator_tracer', outputs) robot.device.after.addSignal('base_estimator.q') # --- RUN SIMULATION plug(robot.comTrajGen.x, robot.taskCom.featureDes.errorIN) sleep(1.0) os.system("rosservice call \start_dynamic_graph") sleep(2.0) robot.comTrajGen.move(1, -0.025, 4.0) sleep(5.0) robot.comTrajGen.startSinusoid(1, 0.05, 8.0) sleep(0.2) robot.device_tracer.start() robot.estimator_tracer.start() sleep(3.0) dump_tracer(robot.device_tracer) dump_tracer(robot.estimator_tracer) print('data dumped') # --- DISPLAY device_data = read_tracer_file('/tmp/dg_' + robot.device.name + '-robotState.dat') estimator_data = read_tracer_file('/tmp/dg_' + robot.base_estimator.name + '-q.dat') plot_select_traj(device_data, [10, 23, 15]) plot_select_traj(estimator_data, [10, 23, 15]) write_pdf_graph('/tmp/')
def newSot(name): sot_keep = SOT (name) sot_keep.setSize(robot.dynamic.getDimension()) sot_keep.damping.value = 0.001 return name
robot.dynamic.Jwrist_3_joint, I4) task_wrist = Task('wrist_task') task_wrist.controlGain.value = 1 task_wrist.add(feature_wrist.name) #Create tracer tracer = TracerRealTime('trace') tracer.setBufferSize(2**20) tracer.open('/tmp/', 'dg_', '.dat') # Make sure signals are recomputed even if not used in the control graph robot.device.after.addSignal('{0}.triger'.format(tracer.name)) addTrace(robot.device, tracer, robot.device.name, "state") addTrace(robot.device, tracer, feature_wrist._feature.name, "position") addTrace(robot.device, tracer, feature_wrist._reference.name, "position") # solver solver = SOT('solver') solver.setSize(dimension) solver.push(task_waist.name) solver.push(task_wrist.name) solver.push(task_elbow.name) solver.push(task_lift.name) solver.push(task_wrist1.name) solver.push(task_wrist2.name) plug(solver.control, robot.device.control) #robot.device.increment (0.01) dt = 0.01 from dynamic_graph.sot.core.utils.thread_interruptible_loop import loopInThread, loopShortcuts
def main(robot): robot.timeStep = robot.device.getTimeStep() dt = robot.timeStep # --- 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 robot.estimator = create_dummy_dcm_estimator(robot) # --- Admittance controller Kp = [0.0, 0.0, 0.0] robot.com_admittance_control = create_com_admittance_controller( Kp, dt, robot) # --- 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 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.contactLF.task.name) robot.sot.push(robot.taskCom.task.name) robot.device.control.recompute(0) # --- TRACER robot.tracer = TracerRealTime("zmp_tracer") robot.tracer.setBufferSize(80 * (2**20)) robot.tracer.open('/tmp', 'dg_', '.dat') robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name)) addTrace(robot.tracer, robot.dynamic, 'zmp') addTrace(robot.tracer, robot.estimator, 'dcm') # SIMULATION plug(robot.com_admittance_control.comRef, robot.taskCom.featureDes.errorIN) sleep(1.0) os.system("rosservice call \start_dynamic_graph") sleep(2.0) plug(robot.estimator.dcm, robot.com_admittance_control.zmpDes) robot.com_admittance_control.setState(robot.dynamic.com.value, [0.0, 0.0, 0.0]) robot.com_admittance_control.Kp.value = [10.0, 10.0, 0.0] robot.tracer.start() sleep(5.0) dump_tracer(robot.tracer) # --- DISPLAY zmp_data = np.loadtxt('/tmp/dg_' + robot.dynamic.name + '-zmp.dat') zmpDes_data = np.loadtxt('/tmp/dg_' + robot.estimator.name + '-dcm.dat') plt.figure() plt.plot(zmp_data[:, 1], 'b-') plt.plot(zmpDes_data[:, 1], 'b--') plt.plot(zmp_data[:, 2], 'r-') plt.plot(zmpDes_data[:, 2], 'r--') plt.title('ZMP real vs desired') plt.legend(['Real x', 'Desired x', 'Real y', 'Desired y']) plt.figure() plt.plot(zmp_data[:, 1] - zmpDes_data[:, 1], 'b-') plt.plot(zmp_data[:, 2] - zmpDes_data[:, 2], 'r-') plt.title('ZMP error') plt.legend(['Error on x', 'Error on y']) plt.show()
def makeTransition(self, stateFrom, stateTo, ig): sf = stateFrom st = stateTo names = self._transitionNames(sf, st, ig) print "names: {}".format(names) iobj = self.objectFromHandle[st.grasps[ig]] obj = self.objects[iobj] noPlace = self._isObjectGrasped(sf.grasps, iobj) print "iobj, obj, noPlace: {}, {}, {}".format(iobj, obj, noPlace) # The different cases: pregrasp = True intersec = not noPlace preplace = not noPlace # Start here nWaypoints = pregrasp + intersec + preplace nTransitions = 1 + nWaypoints # Link waypoints transitions = names[:] assert nWaypoints > 0 M = 1 + pregrasp sots = [] for i in range(nTransitions): ns = ("{0}_{1}{2}".format(names[0], i, i + 1), "{0}_{2}{1}".format(names[1], i, i + 1)) for n in ns: s = SOT('sot_' + n) s.setSize(self.sotrobot.dynamic.getDimension()) self.hpTasks.pushTo(s) #if pregrasp and i == 1: # Add pregrasp task #self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'pregrasp').pushTo (s) if i < M: sf.manifold.pushTo(s) else: st.manifold.pushTo(s) self.lpTasks.pushTo(s) self.sots[n] = s sots.append(n) key = sots[2 * (M - 1)] states = (st.name, names[0] + "_intersec") sot = SOT("postAction_" + key) sot.setSize(self.sotrobot.dynamic.getDimension()) self.hpTasks.pushTo(sot) # self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot) st.manifold.pushTo(sot) self.lpTasks.pushTo(sot) # print sot # TODO one post action is missing if not self.postActions.has_key(key): self.postActions[key] = dict() for n in states: self.postActions[key][n] = sot key = sots[2 * (M - 1) + 1] states = (st.name, names[0] + "_intersec") sot = SOT("preAction_" + key) sot.setSize(self.sotrobot.dynamic.getDimension()) self.hpTasks.pushTo(sot) # self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot) sf.manifold.pushTo(sot) self.lpTasks.pushTo(sot) # print sot self.preActions[key] = sot self.preActions[sots[2 * (M - 1)]] = sot