def setUp(self):
Test_simple_3R.setUp(self)
self.joints["Shoulder"].gpos[:] = pi / 2.
self.joints["Elbow"].gpos[:] = pi / 6.
self.joints["Wrist"].gpos[:] = pi / 3.
self.tasks = [TorqueTask(self.joints["Shoulder"], ValueCtrl(.1))]
self.gforcemax = {"Shoulder": 10, "Elbow": 5, "Wrist": 2}
self.dgforcemax = {}
self.qlim = {}
self.vlim = {}
self.options = {}
# #
#################################
w = create_3r_and_init(gpos=(.5, .5, .5))
joints = w.getjoints()
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## 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 #
[joints['head_' +n] for n in ['pitch', 'roll', 'yaw']]
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## TASKS
from LQPctrl.task import MultiJointTask, ForceTask, FrameTask, CoMTask
from LQPctrl.task_ctrl import KpCtrl, ValueCtrl, KpTrajCtrl
tasks = NamedObjectsList([])
tasks.append(MultiJointTask(icub_joints, KpCtrl(sitting_pose, 10), [], 1e-3 , 0, True, "sitting_pose"))
tasks.append(MultiJointTask(icub_joints, KpCtrl(standing_pose, 10), [], 0. , 0, True, "standing_pose"))
tasks.append(MultiJointTask(spine_joints, KpCtrl(zeros(len(spine_joints)), 10), [], 1e-2 , 0, True, "spine"))
tasks.append(ForceTask( const["l_gluteal"], ValueCtrl([0, 0, 0]) , [], 0., 0, True, "l_gluteal_force"))
tasks.append(ForceTask( const["r_gluteal"], ValueCtrl([0, 0, 0]) , [], 0., 0, True, "r_gluteal_force"))
tasks.append(FrameTask( frames["l_hip_2"], KpTrajCtrl([None,None,zeros((1,6))], 10), [3,4,5], 1., 0, True, "l_gluteal_acc"))
tasks.append(FrameTask( frames["r_hip_2"], KpTrajCtrl([None,None,zeros((1,6))], 10), [3,4,5], 1., 0, True, "r_gluteal_acc"))
tasks.append(CoMTask(icub_bodies, KpCtrl([0, 0, 0], 10), [0, 1], 0., 0, name='com'))
## EVENTS
from LQPctrl.event import Event, AtT, Prtr, SetF, IfF, ChW, InCH, CAct, DelayF
events = []
tt=.25
feet_frames = [frames[n] for n in ['lf1', 'lf2', 'lf3', 'lf4', 'rf1', 'rf2', 'rf3', 'rf4']]
events.append(Event(AtT(.0),
SetF("start lifting", False)))
events.append(Event(AtT(.1),
SetF("avance com", True)))
events.append(Event(InCH(feet_frames, 'CoM', [0,1]),
def setUp(self):
Test_3R_with_Plane_LQPCtrl.setUp(self)
self.tasks.append(
ForceTask(self.const['const'], ValueCtrl([0, 0, 0]), [], 1., 0,
True))
def setUp(self):
Test_3R_LQPCtrl.setUp(self)
self.tasks.append(
MultiTorqueTask(self.joints, ValueCtrl([.03, .02, .01]), [], 1., 0,
True))
def setUp(self):
Test_3R_LQPCtrl.setUp(self)
self.tasks.append(
TorqueTask(self.joints["Shoulder"], ValueCtrl(-.1), [], 1., 0,
True))
## TASKS
from LQPctrl.task import MultiJointTask, ForceTask, FrameTask, CoMTask
from LQPctrl.task_ctrl import KpCtrl, ValueCtrl, KpTrajCtrl
tasks = NamedObjectsList([])
tasks.append(
MultiJointTask(icub_joints, KpCtrl(sitting_pose, 10), [], 1e-3, 0, True,
"sitting_pose"))
tasks.append(
MultiJointTask(icub_joints, KpCtrl(standing_pose, 10), [], 0., 0, True,
"standing_pose"))
tasks.append(
MultiJointTask(spine_joints, KpCtrl(zeros(len(spine_joints)), 10), [],
1e-2, 0, True, "spine"))
tasks.append(
ForceTask(const["l_gluteal"], ValueCtrl([0, 0, 0]), [], 0., 0, True,
"l_gluteal_force"))
tasks.append(
ForceTask(const["r_gluteal"], ValueCtrl([0, 0, 0]), [], 0., 0, True,
"r_gluteal_force"))
tasks.append(
FrameTask(frames["l_hip_2"],
KpTrajCtrl([None, None, zeros((1, 6))],
10), [3, 4, 5], 1., 0, True, "l_gluteal_acc"))
tasks.append(
FrameTask(frames["r_hip_2"],
KpTrajCtrl([None, None, zeros((1, 6))],
10), [3, 4, 5], 1., 0, True, "r_gluteal_acc"))
tasks.append(
CoMTask(icub_bodies, KpCtrl([0, 0, 0], 10), [0, 1], 0., 0, name='com'))
# #
#################################
w = create_3r_and_init(gpos=(.5, .5, .5))
joints = w.getjoints()
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## TASKS
from LQPctrl.task import MultiTorqueTask
from LQPctrl.task_ctrl import ValueCtrl
tasks = []
goal = [.03, .02, .01]
tasks.append(MultiTorqueTask(joints, ValueCtrl(goal), [], 1., 0, True))
## EVENTS
events = []
## LQP CONTROLLER
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = {"Shoulder": 10, "Elbow": 5, "Wrist": 2}
lqpc = LQPcontroller(gforcemax, tasks=tasks)
w.register(lqpc)
############################
# #
# SET OBSERVERS & SIMULATE #
# #
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## TASKS
from LQPctrl.task import ForceTask, FrameTask
from LQPctrl.task_ctrl import ValueCtrl, KpCtrl
from arboris.homogeneousmatrix import rotz, transl
from numpy import pi
tasks = []
goal = rotz(pi / 8)
goal[0:3, 3] = [-0.4, .5, 0]
goal = transl(-.8, -.5, 0)
tasks.append(
ForceTask(const["const"], ValueCtrl([0, 0.001, .01]), [], 1., 0, True))
## EVENTS
events = []
## LQP CONTROLLER
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = {"Shoulder": 10, "Elbow": 5, "Wrist": 2}
lqpc = LQPcontroller(gforcemax, tasks=tasks)
w.register(lqpc)
############################
# #
# SET OBSERVERS & SIMULATE #
# #