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")) #from arboris.homogeneousmatrix import rotz, rotx #from numpy import pi from numpy import array, cos, sin, pi hand_goal = array([[-1, 0, 0, -0.2], [0, -1, 0, 0], [0, 0, 1, 0.6], [0, 0, 0, 1]]) tasks.append( FrameTask(frames['l_hand_palm'], KpCtrl(hand_goal, 10), [], 1, 0, True, "hand")) ## EVENTS events = [] ## LQP CONTROLLER from LQPctrl.LQPctrl import LQPcontroller gforcemax = icub.get_torque_limits() data = {} data = { 'collision shapes': [(shapes['l_hand'], shapes['obstacle']), (shapes['l_hand'], shapes['obstacle2'])] } opt = {'avoidance horizon': 1.5} sopt = {"show_progress": False}
def setUp(self): Test_WithKpCtrl.setUp(self) self.tasks.append(FrameTask(self.frames["EndEffector"], KpCtrl(eye(4), 20), [3,4],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.} sopt = {"show_progress": False}
######################################### # # # 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]), [SetF("in CH", True)] )) events.append(Event(IfF("avance com", True),
def setUp(self): Test_3R_LQPCtrl.setUp(self) self.tasks.append( FrameTask(self.frames["EndEffector"], KpCtrl(eye(4), 10), [], 1., 0, True))
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) obs = [ PerfMonitor(True), RecordJointPosition(joints), RecordFramePosition(frames["EndEffector"])
######################################### # # # 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 # # #