def simulate(self): self.lqpc = LQPcontroller(self.gforcemax, self.dgforcemax, qlim=self.qlim, vlim=self.vlim, tasks=self.tasks, options=self.options, solver_options={"show_progress": False}) self.w.register(self.lqpc) simulate(self.w, arange(0, .04, .01), [])
events = [] ## LQP CONTROLLER from LQPctrl.LQPctrl import LQPcontroller gforcemax = icub.get_torque_limits() data = {'bodies': icub_bodies} data.update({ 'collision shapes': [(shapes['l_hand_palm'], shapes['wall']), (shapes['r_hand_palm'], shapes['wall'])] }) opt = {'avoidance horizon': .1} sopt = {"show_progress": False} lqpc = LQPcontroller(gforcemax, tasks=tasks, data=data, options=opt, solver_options=sopt) w.register(lqpc) w.register(KpTable(bodies['wall'], 0.05, 2., 100.)) ############################ # # # SET OBSERVERS & SIMULATE # # # ############################ obs = get_usual_observers(w) ## SIMULATE from numpy import arange from arboris.core import simulate
def simulate(self, options): self.lqpc = LQPcontroller(self.gforcemax, tasks=self.tasks, options=options, solver_options={"show_progress":False}) self.w.register(self.lqpc) simulate(self.w, arange(0, 4., .01), []) self.check_results()
## TASKS from LQPctrl.task import JointTask from LQPctrl.task_ctrl import KpCtrl tasks = [] goal = .1 tasks.append(JointTask(joints["Shoulder"], KpCtrl(goal, 10), [], 1., 0, True)) ## EVENTS events = [] from LQPctrl.LQPctrl import LQPcontroller gforcemax = {"Elbow": 5, "Wrist": 2} #"Shoulder":10, # ...or... #gforcemax = {"Shoulder":0,"Elbow":5,"Wrist":2} lqpc = LQPcontroller(gforcemax, tasks=tasks) w.register(lqpc) ############################ # # # SET OBSERVERS & SIMULATE # # # ############################ obs = get_usual_observers(w) from common import RecordJointPosition obs.append(RecordJointPosition(joints["Shoulder"])) ## SIMULATE from numpy import arange from arboris.core import simulate
## TASKS from LQPctrl.task import TorqueTask from LQPctrl.task_ctrl import ValueCtrl tasks = [] goal = .1 tasks.append(TorqueTask(joints["Shoulder"], ValueCtrl(goal), [], 1., 0, True)) ## EVENTS events = [] ## LQP CONTROLLER from LQPctrl.LQPctrl import LQPcontroller gforcemax = {"Shoulder": 10, "Elbow": 5, "Wrist": 2} opt = {'base weights': (1e-10, 1e-10, 1e-10)} lqpc = LQPcontroller(gforcemax, tasks=tasks, options=opt) w.register(lqpc) ############################ # # # SET OBSERVERS & SIMULATE # # # ############################ obs = get_usual_observers(w) from common import RecordGforce obs.append(RecordGforce(lqpc)) ## SIMULATE from numpy import arange from arboris.core import simulate
ChW(tasks['standing_pose'], 1e-3, tt), ChW(tasks['l_gluteal_acc'], 0, tt), ChW(tasks['r_gluteal_acc'], 0, tt), DelayF("can del sitting", True, tt), SetF("can del const", False)])) events.append(Event(IfF("can del sitting", True), [ChW(tasks['sitting_pose'], 0., tt), SetF("can del sitting", False)])) ## LQP CONTROLLER from LQPctrl.LQPctrl import LQPcontroller gforcemax = icub.get_torque_limits() opt = {'base weights' : (1e-7, 1e-7, 1e-7)} sopt = {'show_progress':False, 'abstol' : 1e-12, 'reltol' : 1e-12, 'feastol' : 1e-12,} lqpc = LQPcontroller(gforcemax, tasks=tasks, events=events, options = opt, solver_options=sopt) w.register(lqpc) ############################ # # # SET OBSERVERS & SIMULATE # # # ############################ obs = get_usual_observers(w) from common import RecordCoMPosition, RecordGforce obs.append(RecordGforce(lqpc)) obs.append(RecordCoMPosition(icub_bodies))
w = create_3r_and_init(gpos=gpos) frames = w.getframes() joints = w.getjoints() tasks = [] tasks.append(MultiJointTask(joints, KpCtrl(gpos, 10), [], .001, 0, True)) ee_goal = eye(4) ee_goal[0:3, 3] = goal[g] tasks.append( FrameTask(frames["EndEffector"], KpCtrl(ee_goal, 10), [3, 4], 1., 0, True)) lqpc = LQPcontroller(gforcemax, tasks=tasks, options={ 'cost': c, "norm": n, "formalism": f }, solver_options={"show_progress": False}) w.register(lqpc) obs = [ PerfMonitor(True), RecordJointPosition(joints), RecordFramePosition(frames["EndEffector"]) ] simulate(w, arange(0, 3., 0.01), obs) jtraj[opt] = obs[-2].get_record() traj[opt] = obs[-1].get_record() results[opt] = lqpc.get_performance()
############### SIMULATIONS ################### results = {} traj = {} for opt in options: print """=============================================== START : {0} =============================================== """.format(opt) w = create_3r_and_init(gpos=(.5,.5,.5)) frames = w.getframes() tasks = [] tasks.append(FrameTask(frames["EndEffector"], KpCtrl(goal, 20), [], 1., 0, True)) c,n,f = opt.split(";") lqpc = LQPcontroller(gforcemax, tasks=tasks, options={'cost':c,"norm":n,"formalism":f}, solver_options={"show_progress":False}) w.register(lqpc) obs = [PerfMonitor(True), RecordFramePosition(frames["EndEffector"])] timeline = arange(0,3.,0.01) simulate(w, timeline, obs) traj[opt] = obs[-1].get_record() results[opt] = lqpc.get_performance() ############### RESULTS ################### pl.figure() for k in traj: pl.plot(traj[k])