# #
#################################
w = create_3r_and_init(gpos=(.5, .5, .5))
joints = w.getjoints()
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## 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 #
def setUp(self):
Test_WithKpCtrl.setUp(self)
self.tasks.append(JointTask(self.joints["Shoulder"], KpCtrl(0.1, 20), [], 1., 0, True))
self.tasks.append(JointTask(self.joints["Elbow"], KpCtrl(0.1, 20), [], 1., 0, True))
self.tasks.append(JointTask(self.joints["Wrist"], KpCtrl(0.1, 20), [], 1., 0, True))
#w.register(SoftFingerContact((shapes['ee'], shapes['obstacle']), .01))
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## TASKS
from LQPctrl.task import FrameTask
from LQPctrl.task_ctrl import KpCtrl
from arboris.homogeneousmatrix import rotz
from numpy import pi
tasks = []
goal = rotz(3 * pi / 4)
goal[0:3, 3] = [-.75, -.0, 0]
tasks.append(FrameTask(frames["EndEffector"], KpCtrl(goal, 2), [], 1., 0,
True))
## EVENTS
events = []
## LQP CONTROLLER
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = {"Shoulder": 10, "Elbow": 5, "Wrist": 2}
data = {}
data = {
'collision shapes': [(shapes['ee'], shapes['obstacle']),
(shapes['ee'], shapes['obstacle2'])]
}
opt = {'avoidance horizon': 1.}
icub_joints = [joints[n] for n in icub.get_joints_data()]
standing_pose = [j.gpos[0] for j in icub_joints]
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## TASKS
from LQPctrl.task import MultiJointTask
from LQPctrl.task_ctrl import KpCtrl
from arboris.homogeneousmatrix import rotz
tasks = []
tasks.append(
MultiJointTask(icub_joints, KpCtrl(standing_pose, 10), [], 1, 0, True,
"standing_pose"))
## EVENTS
events = []
## LQP CONTROLLER
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = icub.get_torque_limits()
lqpc = LQPcontroller(gforcemax, tasks=tasks)
w.register(lqpc)
############################
# #
# SET OBSERVERS & SIMULATE #
def setUp(self):
Test_3R_LQPCtrl.setUp(self)
self.tasks.append(LQPCoMTask(KpCtrl([0, 0, 0], 10), [], 1., 0, True))
w.update_dynamic()
spine_joints = [joints['torso_'+n] for n in ['pitch', 'roll', 'yaw']] + \
[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)))
def setUp(self):
Test_3R_LQPCtrl.setUp(self)
self.tasks.append(
MultiJointTask(self.joints, KpCtrl([.1, .1, .1], 10), [], 1., 0,
True))
w.update_dynamic()
spine_joints = [joints['torso_'+n] for n in ['pitch', 'roll', 'yaw']] + \
[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(spine_joints, KpCtrl(zeros(len(spine_joints)), 10), [], 1e-2 , 0, True, "spine"))
## EVENTS
events = []
## LQP CONTROLLER
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = icub.get_torque_limits()
lqpc = LQPcontroller(gforcemax, tasks=tasks)
w.register(lqpc)
############################
traj = {}
results = {}
for opt in options:
print """===============================================
START : {0}
===============================================
""".format(opt)
c, n, f, g = opt.split("_")
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)
def setUp(self):
Test_3R_LQPCtrl.setUp(self)
self.tasks.append(
JointTask(self.joints["Shoulder"], KpCtrl(0.1, 10), [], 1., 0,
True))
bodies = w.getbodies()
arm_bodies = [bodies[n] for n in ['Arm', 'Forearm', 'Hand']]
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## TASKS
from LQPctrl.task import JointTask, CoMTask, LQPCoMTask
from LQPctrl.task_ctrl import KpCtrl
from arboris.homogeneousmatrix import rotz
tasks = []
goal = [-0.4, .2, 0]
tasks.append(JointTask(joints["Wrist"], KpCtrl(.5, 10.), [], 1., 0, True))
#tasks.append(CoMTask(arm_bodies, KpCtrl(goal, 10.), [], 1., 0, True))
# ...or...
tasks.append(LQPCoMTask(KpCtrl(goal, 10.), [], 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)
############################
spine_joints = [joints['torso_'+n] for n in ['pitch', 'roll', 'yaw']] + \
[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"],
# #
#################################
w = create_3r_and_init(gpos=(.5, .4, .3))
joints = w.getjoints()
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## TASKS
from LQPctrl.task import MultiJointTask
from LQPctrl.task_ctrl import KpCtrl
tasks = []
goal = [.1, .1, .1]
tasks.append(MultiJointTask(joints, KpCtrl(goal, 10), [], 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 #
# #
def setUp(self):
Test_WithKpCtrl.setUp(self)
self.tasks.append(FrameTask(self.frames["EndEffector"], KpCtrl(eye(4), 20), [3,4],1.,0, True))
def setUp(self):
Test_3R_LQPCtrl.setUp(self)
self.tasks.append(
FrameTask(self.frames["EndEffector"], KpCtrl(eye(4), 10), [], 1.,
0, True))
icub_bodies = [bodies[n] for n in icub.get_bodies_data()]
standing_pose = [j.gpos[0] for j in icub_joints]
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## TASKS
from LQPctrl.task import MultiJointTask, FrameTask, JointTask
from LQPctrl.task_ctrl import KpCtrl
from arboris.homogeneousmatrix import rotz
tasks = []
tasks.append(
MultiJointTask(icub_joints, KpCtrl(standing_pose, 10), [], .01, 0, True,
"standing_pose"))
from numpy import eye
root_goal = eye(4)
root_goal[0:3, 3] = [0, 0, .59]
tasks.append(
JointTask(joints['root'], KpCtrl(root_goal, 10), [], 1, 0, True, "root"))
spine_joints = [joints['torso_' + n] for n in ['pitch', 'roll', 'yaw']]
tasks.append(
MultiJointTask(spine_joints, KpCtrl([0, 0, 0], 10), [], 1, 0, True,
"spine"))
## EVENTS
events = []
## LQP CONTROLLER
#########################################
# #
# CREATE TASKS, EVENTS & LQP controller #
# #
#########################################
## TASKS
from LQPctrl.task import FrameTask
from LQPctrl.task_ctrl import KpCtrl
from arboris.homogeneousmatrix import rotz
from numpy import pi
tasks = []
goal = rotz(pi / 8)
goal[0:3, 3] = [-0.4, .5, 0]
tasks.append(
FrameTask(frames["EndEffector"], KpCtrl(goal, 20), [], 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 #
# #