Python JointTask Exemples

Exemple #1

#                                       #
#########################################
## 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
from LQPctrl.LQPctrl import LQPcontroller
gforcemax = icub.get_torque_limits()

data = {'bodies': icub_bodies}
data.update({
    'collision shapes': [(shapes['l_hand_palm'], shapes['wall']),

Exemple #2

Fichier : test_KpCtrl.py Projet : salini/LQPctrl

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

Exemple #3

Fichier : test_cost_norm_formalism.py Projet : salini/LQPctrl

 def setUp(self):
     Test_3R_LQPCtrl.setUp(self)
     self.tasks.append(
         JointTask(self.joints["Shoulder"], KpCtrl(0.1, 10), [], 1., 0,
                   True))

Exemple #4

#                               #
#################################
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 #

Exemple #5

Fichier : 08_CoM_control.py Projet : salini/LQPctrl

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)

############################