#!/usr/bin/python #coding=utf-8 #author=Joseph Salini #date=16 may 2011 from common import create_3r_and_init, get_usual_observers ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# 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
#!/usr/bin/python # coding=utf-8 # author=Joseph Salini # date=16 may 2011 from common import create_3r_and_init, get_usual_observers ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# w = create_3r_and_init(gpos=(0.5, 0.5, 0.5)) frames = w.getframes() ######################################### # # # 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, 0.5, 0] tasks.append(FrameTask(frames["EndEffector"], KpCtrl(goal, 20), [], 1.0, 0, True))
#!/usr/bin/python #coding=utf-8 #author=Joseph Salini #date=16 may 2011 from common import create_3r_and_init, add_plane_and_point_on_arm, get_usual_observers from numpy import pi, array ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# w = create_3r_and_init(gpos=(pi/2.,pi/6.,pi/3.), gravity=False) add_plane_and_point_on_arm(w, (0,1,0,-.4)) frames = w.getframes() shapes = w.getshapes() const = w.getconstraints() ######################################### # # # 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
#!/usr/bin/python # coding=utf-8 # author=Joseph Salini # date=25 may 2011 from common import create_3r_and_init, get_usual_observers ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# w = create_3r_and_init(gpos=(0.7, 0.6, 0.5)) joints = w.getjoints() ######################################### # # # CREATE TASKS, EVENTS & LQP controller # # # ######################################### ## TASKS from LQPctrl.task import MultiJointTask from LQPctrl.task_ctrl import KpCtrl tasks = [] goal = [0.1, 0.1, 0.1] tasks.append(MultiJointTask(joints, KpCtrl(goal, 10), [], 1.0, 0, True)) ## EVENTS
#!/usr/bin/python #coding=utf-8 #author=Joseph Salini #date=16 may 2011 from common import create_3r_and_init, get_usual_observers ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# w = create_3r_and_init(gpos=(.5,.5,.5)) frames = w.getframes() dt = 0.01 ######################################### # # # CREATE TASKS, EVENTS & LQP controller # # # ######################################### ## TASKS from LQPctrl.task import FrameTask from LQPctrl.task_ctrl import KpTrajCtrl from arboris.homogeneousmatrix import rotz from numpy import pi, tile, cos, sin, zeros, arange tasks = [] amp, T, phi = .5, 4., pi/2
for g in ['inside', 'limit', 'beyond']: options.append(cost + "_" + norm + "_" + form + "_" + g) ############### SIMULATIONS ################### jtraj = {} 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,
#!/usr/bin/python #coding=utf-8 #author=Joseph Salini #date=25 may 2011 from common import create_3r_and_init, get_usual_observers ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# 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
options.append(cost+"_"+norm+"_"+form+"_"+g) ############### SIMULATIONS ################### jtraj ={} 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) obs = [PerfMonitor(True), RecordJointPosition(joints), RecordFramePosition(frames["EndEffector"])] simulate(w, arange(0,3.,0.01), obs)
#!/usr/bin/python #coding=utf-8 #author=Joseph Salini #date=25 may 2011 from common import create_3r_and_init, get_usual_observers ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# 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 = []
#!/usr/bin/python #coding=utf-8 #author=Joseph Salini #date=16 may 2011 from common import create_3r_and_init, add_plane_and_point_on_arm, get_usual_observers from numpy import pi, array ################################# # # # CREATE WORLD & INITIALIZATION # # # ################################# w = create_3r_and_init(gpos=(pi / 2., pi / 6., pi / 3.), gravity=False) add_plane_and_point_on_arm(w, (0, 1, 0, -.4)) frames = w.getframes() shapes = w.getshapes() const = w.getconstraints() ######################################### # # # 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)