# # ######################################### ## 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']),
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))
def setUp(self): Test_3R_LQPCtrl.setUp(self) self.tasks.append( JointTask(self.joints["Shoulder"], KpCtrl(0.1, 10), [], 1., 0, True))
# # ################################# 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 #
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) ############################