Skip to content
Snippets Groups Projects
Commit 7a9738c6 authored by flforget's avatar flforget
Browse files

add COM trajectory generator

test COM trajectory tracking in gazebo
parent a30396af
No related branches found
No related tags found
No related merge requests found
......@@ -144,6 +144,7 @@ IF(BUILD_PYTHON_INTERFACE)
INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/test/__init__.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_jointTrajGen.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_jointControl.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj.py
DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME}/test)
ENDIF(BUILD_TEST)
ENDIF(BUILD_PYTHON_INTERFACE)
......
......@@ -12,6 +12,13 @@ def create_joint_trajectory_generator(dt):
jtg.init(dt, N_JOINTS);
return jtg;
def create_com_trajectory_generator(dt,robot):
comTrajGen = NdTrajectoryGenerator("comTrajGen");
comTrajGen.initial_value.value = robot.dynamic.com.value
comTrajGen.trigger.value = 1.0;
comTrajGen.init(dt, 3);
return comTrajGen;
def create_joint_controller(Kp):
controller = JointPositionController("posctrl")
controller.Kp.value = Kp
......
from sot_talos_balance.create_entities_utils import create_com_trajectory_generator
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
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
def main(robot):
dt = robot.timeStep;
robot.comTrajGen = create_com_trajectory_generator(dt,robot);
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
robot.taskCom.task.controlGain.value = 10
# --- 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.taskCom.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.device.control.recompute(0)
plug(robot.comTrajGen.x, robot.taskCom.featureDes.errorIN);
sleep(1.0);
os.system("rosservice call \start_dynamic_graph")
sleep(2.0);
robot.comTrajGen.move(1,-0.025,4.0);
sleep(5.0);
robot.comTrajGen.startSinusoid(1,0.05,8.0);
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