Commit 2b13647b authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Fix unitary test for se3_trajectory_generator

parent 2009b21a
......@@ -64,7 +64,7 @@ namespace dynamicgraph {
/* --- CONSTRUCTOR ---- */
PositionController( const std::string & name );
void init(const double& dt,const double &nJoints);
void init(const double& dt,const std::string &robotRef);
void resetIntegral();
......
......@@ -79,14 +79,14 @@ namespace dynamicgraph
makeCommandVoid2(*this, &PositionController::init,
docCommandVoid2("Initialize the entity.",
"Time period in seconds (double)",
"Size of the state vector (double)")));
"Reference to the robot (string)")));
addCommand("resetIntegral",
makeCommandVoid0(*this, &PositionController::resetIntegral,
docCommandVoid0("Reset the integral.")));
}
void PositionController::init(const double& dt,
const double& nJoints)
const std::string& robotRef)
{
if(dt<=0.0)
return SEND_MSG("Timestep must be positive", MSG_TYPE_ERROR);
......@@ -106,7 +106,7 @@ namespace dynamicgraph
return SEND_MSG("Init failed: signal Ki is not plugged", MSG_TYPE_ERROR);
/* Retrieve m_robot_util informations */
std::string localName("control-manager-robot");
std::string localName(robotRef);
if (isNameInRobotUtil(localName))
m_robot_util = getRobotUtil(localName);
else
......
......@@ -29,9 +29,9 @@ idbc.com_ref_acc.value=comRefAcc
# Right foot
rfRefPos=zeros(12)
rfRefPos(0)=1.0
rfRefPos(4)=1.0
rfRefPos(8)=1.0
rfRefPos[0]=1.0
rfRefPos[4]=1.0
rfRefPos[8]=1.0
rfRefVel=zeros(6)
rfRefAcc=zeros(6)
......@@ -41,9 +41,9 @@ idbc.rf_ref_acc.value=rfRefAcc
# Left foot
lfRefPos=zeros(12)
lfRefPos(0)=1.0
lfRefPos(4)=1.0
lfRefPos(8)=1.0
lfRefPos[0]=1.0
lfRefPos[4]=1.0
lfRefPos[8]=1.0
lfRefVel=zeros(6)
lfRefAcc=zeros(6)
idbc.lf_ref_pos.value=lfRefPos
......
from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate, matrixToRPY
from dynamic_graph.sot.torque_control.se3_trajectory_generator import *
from numpy import matrix, identity, zeros, eye, array, pi, ndarray, ones
from dynamic_graph.sot.torque_control.tests.robot_data_test import initRobotData
# Instanciate a pose trajectory generator
initial_value=zeros(6)
se3tg=SE3TrajectoryGenerator("se3tg_test")
se3tg.init(initRobotData.controlDT)
se3tg.initial_value.value = initial_value
se3tg.recompute(10)
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment