Skip to content
Snippets Groups Projects
Commit e130b55a authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

test track single joint

parent b1d4598e
No related branches found
No related tags found
No related merge requests found
......@@ -139,10 +139,12 @@ IF(BUILD_PYTHON_INTERFACE)
INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/__init__.py
python/${SOTTALOSBALANCE_PYNAME}/main.py
python/${SOTTALOSBALANCE_PYNAME}/create_entities_utils.py
python/${SOTTALOSBALANCE_PYNAME}/meta_task_config.py
DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME})
IF(BUILD_TEST)
INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/test/__init__.py
python/${SOTTALOSBALANCE_PYNAME}/test/run_test_utils.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_singleTraj.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_jointTrajGen.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_jointControl.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj.py
......
......@@ -12,6 +12,14 @@ def create_joint_trajectory_generator(dt):
jtg.init(dt, N_JOINTS);
return jtg;
def create_config_trajectory_generator(dt):
N_CONFIG = N_JOINTS + 6
jtg = NdTrajectoryGenerator("jtg");
jtg.initial_value.value = tuple(N_CONFIG*[0.0]);
jtg.trigger.value = 1.0;
jtg.init(dt, N_CONFIG);
return jtg;
def create_com_trajectory_generator(dt,robot):
comTrajGen = NdTrajectoryGenerator("comTrajGen");
comTrajGen.initial_value.value = robot.dynamic.com.value
......
from dynamic_graph.sot.core.meta_task_6d import toFlags
from dynamic_graph import plug
from dynamic_graph.sot.core import *
# from dynamic_graph.sot.core.meta_tasks import setGain
# from dynamic_graph.sot.dyninv import *
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate
from numpy import matrix, identity, zeros, eye
class MetaTaskConfig(object):
nbDof = None
def __init__(self,dyn,config,name="joint"):
self.dyn=dyn
self.name=name
self.config = config
self.feature = FeatureGeneric('feature'+name)
self.featureDes = FeatureGeneric('featureDes'+name)
self.gain = GainAdaptive('gain'+name)
plug(dyn.position,self.feature.errorIN)
robotDim = len(dyn.position.value)
self.feature.jacobianIN.value = matrixToTuple( identity(robotDim) )
self.feature.setReference(self.featureDes.name)
self.feature.selec.value = toFlags(self.config)
def plugTask(self):
self.task.add(self.feature.name)
plug(self.task.error,self.gain.error)
plug(self.gain.gain,self.task.controlGain)
@property
def ref(self):
return self.featureDes.errorIN.value
@ref.setter
def ref(self,v):
self.featureDes.errorIN.value = v
class MetaTaskKineConfig(MetaTaskConfig):
def __init__(self,dyn,config,name="joint"):
MetaTaskConfig.__init__(self,dyn,config,name)
self.task = Task('task'+name)
self.plugTask()
......@@ -31,7 +31,7 @@ def main(robot,gain):
sleep(1.0)
robot.traj_gen.move(31,-1.0,1.0)
sleep(1.1)
robot.traj_gen.startSinusoid(31,3.0,2.0)
robot.traj_gen.startSinusoid(31,1.0,2.0)
sleep(10.0)
robot.traj_gen.stop(31)
......
......@@ -17,6 +17,6 @@ def main(robot):
sleep(1.0);
robot.traj_gen.move(31,-1.0,1.0);
sleep(1.1);
robot.traj_gen.startSinusoid(31,3.0,2.0);
robot.traj_gen.startSinusoid(31,1.0,2.0);
sleep(10.0);
robot.traj_gen.stop(31);
from sot_talos_balance.create_entities_utils import create_config_trajectory_generator
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d
from sot_talos_balance.meta_task_config import MetaTaskKineConfig
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
from numpy import eye
from time import sleep
import os
N_JOINTS = 32
N_CONFIG = N_JOINTS + 6
def main(robot):
dt = robot.timeStep;
robot.traj_gen = create_config_trajectory_generator(dt)
JOINT = 31
QJOINT = JOINT + 6
# --- Joint
robot.taskJoint = MetaTaskKineConfig(robot.dynamic,[QJOINT])
# robot.dynamic.com.recompute(0)
robot.taskJoint.featureDes.errorIN.value = N_CONFIG*[0.0]
robot.taskJoint.task.controlGain.value = 100
# --- CONTACTS
#define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(100)
robot.contactLF.keep()
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(100)
robot.contactRF.keep()
locals()['contactRF'] = robot.contactRF
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
plug(robot.sot.control,robot.device.control)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskJoint.task.name)
robot.device.control.recompute(0)
plug(robot.traj_gen.x, robot.taskJoint.featureDes.errorIN)
sleep(1.0)
os.system('rosservice call /start_dynamic_graph')
sleep(1.0)
robot.traj_gen.move(QJOINT,-1.0,1.0)
sleep(1.1);
robot.traj_gen.startSinusoid(QJOINT,1.0,8.0)
sleep(10.0)
robot.traj_gen.stop(QJOINT)
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment