Ejemplo n.º 1
0
#!/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
Ejemplo n.º 2
0
#!/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))
Ejemplo n.º 3
0
#!/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
Ejemplo n.º 4
0
#!/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
Ejemplo n.º 6
0
            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,
Ejemplo n.º 7
0
#!/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
Ejemplo n.º 8
0
                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)
Ejemplo n.º 9
0
#!/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 = []
Ejemplo n.º 10
0
#!/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)