def __init__(self, robot, hands=True): #, forceSeqplay=True):
Application.__init__(self, robot)
self.sot = self.solver.sot
self.hands = hands
# self.robot=robot
# self.seq=Seqplay('seqplay')
self.createTasks(robot)
self.initTasks()
self.initTaskGains()
self.initSolver()
self.initialStack()
def __init__(self,robot,hands=True):#, forceSeqplay=True):
Application.__init__(self,robot)
self.sot=self.solver.sot
self.hands=hands
# self.robot=robot
# self.seq=Seqplay('seqplay')
self.createTasks(robot)
self.initTasks()
self.initTaskGains()
self.initSolver()
self.initialStack()
def __init__(self,robot):
Application.__init__(self,robot)
self.robot = robot
plug(self.robot.dynamic.com,self.robot.device.zmp)
self.sot = self.solver.sot
self.createTasks()
self.initTasks()
self.initTaskGains()
self.initialStack()
def __init__(self,robot,trunkStabilize=False, hands=False, posture=False):
Application.__init__(self,robot)
self.hands =hands
self.posture = posture
self.trunkStabilize = trunkStabilize
self.robot = robot
plug(self.robot.dynamic.com,self.robot.device.zmp)
self.sot = self.solver.sot
self.createTasks()
self.initTasks()
self.initTaskGains()
self.initialStack()
def test_ros(robot):
# SCRIPT PARAMETERS
# j = 0
# index of joint under analysis
# N = 300
# test duration (in number of timesteps)
# dt = 0.001
# time step
# estimationDelay = 10
# delay introduced by the estimation [number of time steps]
createRosTopics = 1
# 1=true, 0=false
# CONSTANTS
nj = 30
# number of joints
# rad2deg = 180 / 3.14
app = Application(robot)
dq_des = nj * (0.0, )
app.robot.device.control.value = dq_des
if (createRosTopics == 1):
ros = RosExport('rosExport')
ros.add('vector', 'robotStateRos', 'state')
plug(robot.device.state, ros.robotStateRos)
# robot.device.after.addSignal('rosExport.robotStateRos')
return ros
def __init__(self,robot,twoHands = True,trunkStabilize = True):
Application.__init__(self,robot)
self.twoHands = twoHands
self.trunkStabilize = trunkStabilize
self.robot = robot
plug(self.robot.dynamic.com,self.robot.device.zmp)
self.sot = self.solver.sot
self.createTasks()
self.initTasks()
self.initTaskGains()
self.initialStack()
def __init__(self,robot,sequenceFile,trunkStabilize=False, hands=False, posture=False, forceSeqplay=True):
Application.__init__(self,robot)
self.hands =hands
self.posture = posture
self.trunkStabilize = trunkStabilize
self.robot = robot
plug(self.robot.dynamic.com,self.robot.device.zmp)
self.sot = self.solver.sot
self.seq = Seqplay ('seqplay')
self.seq.load (sequenceFile)
self.forceSeqplay = forceSeqplay
if self.forceSeqplay:
self.zmpRef = ZmpFromForces('zmpRef')
else:
self.zmpRef = self.seq
self.createTasks()
self.initTasks()
self.initTaskGains()
self.initialStack()
def test_ros(robot):
# SCRIPT PARAMETERS
createRosTopics = 1
# 1=true, 0=false
nj = 30
# number of joints
app = Application(robot)
dq_des = nj * (0.0, )
app.robot.device.control.value = dq_des
if (createRosTopics == 1):
ros = RosImport('rosImport')
ros.add('vector', 'robotStateRos', 'robotState')
plug(robot.device.robotState, ros.robotStateRos)
robot.device.after.addSignal('rosImport.trigger')
return ros
from dynamic_graph.sot.application.velocity.precomputed_tasks import initialize #deprecated
solver=initialize(robot) #deprecated
from dynamic_graph.sot.application.velocity.precomputed_tasks import Application
appli =Application(robot)
robot.initializeTracer()
#launch service start_dynamic_graph
#walk few steps
from dynamic_graph.sot.pattern_generator.walking import CreateEverythingForPG, walkFewSteps, walkFewStepsCircular
CreateEverythingForPG(robot,solver)
walkFewSteps(robot)
#-----------creation of a task---------------
from dynamic_graph.sot.core.meta_tasks import generic6dReference, Task, GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from numpy import *
#show tasks in solver controling the robot
print solver #deprecated
print appli.solver
#show library of precomputed tasks
robot.tasks
#
appli.gains['right-wrist'].setByPoint(4,0.2,0.01,0.8)
trunktask = Task ("trunk-task")
