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])