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

[WIP] unit test of Joint_trajectory_generator entity

parent 67eccfcc
No related branches found
No related tags found
No related merge requests found
......@@ -141,6 +141,7 @@ INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR})
IF(BUILD_PYTHON_INTERFACE)
INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/__init__.py
python/${SOTTALOSBALANCE_PYNAME}/main.py
python/${SOTTALOSBALANCE_PYNAME}/create_entities_utils.py
DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME})
ENDIF(BUILD_PYTHON_INTERFACE)
......
from sot_talos_balance.joint_trajectory_generator import JointTrajectoryGenerator
from sot_talos_balance.joint_position_controller import JointPositionController
from dynamic_graph import plug
def create_joint_trajectory_generator(device, dt=0.001, robot_name="robot"):
jtg = JointTrajectoryGenerator("jtg");
plug(device.robotState, jtg.base6d_encoders);
jtg.init(dt, robot_name);
return jtg;
def create_joint_position_controller(robot, dt=0.001, robot_name="robot"):
jpc = JointPositionController("jpc");
plug(robot.device.robotState, jpc.q);
return jpc;
from sot_talos_balance.joint_trajectory_generator import JointTrajectoryGenerator
from dynamic_graph import plug
dt = robot.timeStep;
robot.traj_gen = JointTrajectoryGenerator("jtg");
plug(robot.device.robotState, robot.traj_gen.base6d_encoders);
robot.traj_gen.init(dt, "robot");
plug(robot.traj_gen.dq, robot.device.control);
robot.device.control.recompute(0)
#!/usr/bin/python
# -*- coding: utf-8 -*-1
import sys
import rospy
from sot_talos_balance.create_entities_utils import create_joint_trajectory_generator
from sot_talos_balance.create_entities_utils import create_joint_position_controller
from dynamic_graph import plug
from std_srvs.srv import *
from dynamic_graph_bridge.srv import *
from dynamic_graph_bridge_msgs.srv import *
def launchScript(code,title,description = ""):
raw_input(title+': '+description)
rospy.loginfo(title)
rospy.loginfo(code)
for line in code:
if line != '' and line[0] != '#':
print line
answer = runCommandClient(str(line))
rospy.logdebug(answer)
print answer
rospy.loginfo("...done with "+title)
# Waiting for services
try:
rospy.loginfo("Waiting for run_command")
rospy.wait_for_service('/run_command')
rospy.loginfo("...ok")
rospy.loginfo("Waiting for start_dynamic_graph")
rospy.wait_for_service('/start_dynamic_graph')
rospy.loginfo("...ok")
runCommandClient = rospy.ServiceProxy('run_command', RunCommand)
runCommandStartDynamicGraph = rospy.ServiceProxy('start_dynamic_graph', Empty)
initCode = open( "appli_jointTrajGen.py", "r").read().split("\n")
rospy.loginfo("Stack of Tasks launched")
# runCommandClient("from dynamic_graph import plug")
# runCommandClient("from dynamic_graph.sot.core import SOT")
# runCommandClient("sot = SOT('sot')")
# runCommandClient("sot.setSize(robot.dynamic.getDimension())")
# runCommandClient("plug(sot.control,robot.device.control)")
dt = robot.timeStep;
launchScript(initCode,'initialize SoT')
raw_input("Wait before starting the dynamic graph")
runCommandStartDynamicGraph()
#raw_input("Wait before moving the hand")
#runCommandClient("target = (0.5,-0.2,1.0)")
#runCommandClient("gotoNd(taskRH,target,'111',(4.9,0.9,0.01,0.9))")
#runCommandClient("sot.push(taskRH.task.name)")
robot.traj_gen = create_joint_trajectory_generator(robot.device,dt)
robot.joint_pos_controller =create_joint_position_controller(robot,dt);
plug(robot.traj_gen.q, robot.joint_pos_controller.qDes);
plug(robot.traj_gen.dq, robot.joint_pos_controller.dqDes);
plug(robot.joint_pos_controller.dqRef, robot.device.control);
robot.joint_pos_controller.init(tuple([1.0]*32));
except rospy.ServiceException, e:
rospy.logerr("Service call failed: %s" % e)
sleep(1.0)
os.system('rosservice call /start_dynamic_graph');
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