Commit 8af4e1f4 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

[nd-trajectory-generator] create setSpline to initialize spline. add trigger...

[nd-trajectory-generator] create setSpline to initialize spline. add trigger to play splines together.
create entity for trajectory synchronization
parent 327d5ea8
......@@ -76,7 +76,8 @@ namespace dynamicgraph {
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(initial_value, dynamicgraph::Vector);
DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(trigger, bool);
DECLARE_SIGNAL(x, OUT, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dx, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(ddx, dynamicgraph::Vector);
......@@ -91,6 +92,9 @@ namespace dynamicgraph {
void playSpline(const std::string& fileName);
void setSpline(const std::string& filename, const double& timeToInitConf);
void startSpline();
/** Print the current value of the specified component. */
void getValue(const int& id);
......@@ -169,6 +173,7 @@ namespace dynamicgraph {
double m_t; /// current control loop time.
unsigned int m_n; /// size of ouput vector
unsigned int m_iterLast; /// last iter index
bool m_splineReady; /// true if the spline has been successfully loaded.
std::vector<JTG_Status> m_status; /// status of the component
std::vector<parametriccurves::AbstractCurve<double, Eigen::Vector1d>* > m_currentTrajGen;
......
......@@ -5,6 +5,7 @@
"""
from dynamic_graph import plug
from dynamic_graph.sot.core.switch import Switch
from dynamic_graph.sot.torque_control.force_torque_estimator import ForceTorqueEstimator
from dynamic_graph.sot.torque_control.numerical_difference import NumericalDifference
from dynamic_graph.sot.torque_control.joint_torque_controller import JointTorqueController
......@@ -92,6 +93,20 @@ def create_com_traj_gen(conf, dt):
com_traj_gen.init(dt,3);
return com_traj_gen ;
def create_force_traj_gen(name, initial_value, dt):
force_traj_gen = NdTrajectoryGenerator(name);
force_traj_gen.initial_value.value = initial_value;
force_traj_gen.init(dt,6);
return force_traj_gen ;
def create_trajectory_switch():
traj_sync = Switch("traj_sync");
return traj_sync ;
def connect_synchronous_trajectories(switch, list_of_traj_gens):
for traj_gen in list_of_traj_gens:
plug(switch.out, traj_gen.trigger);
def create_free_flyer_locator(ent, robot_name="robot"):
from dynamic_graph.sot.torque_control.free_flyer_locator import FreeFlyerLocator
ff_locator = FreeFlyerLocator("ffLocator");
......@@ -296,6 +311,10 @@ def create_balance_controller(robot, conf, motor_params, dt, robot_name='robot')
plug(robot.com_traj_gen.dx, ctrl.com_ref_vel);
plug(robot.com_traj_gen.ddx, ctrl.com_ref_acc);
plug(robot.rf_force_traj_gen.x, ctrl.f_ref_right_foot);
plug(robot.lf_force_traj_gen.x, ctrl.l_ref_right_foot);
# rather than giving to the controller the values of gear ratios and rotor inertias
# it is better to compute directly their product in python and pass the result
# to the C++ entity, because otherwise we get a loss of precision
......
......@@ -6,6 +6,7 @@
from dynamic_graph import plug
from dynamic_graph.sot.torque_control.se3_trajectory_generator import SE3TrajectoryGenerator
from dynamic_graph.sot.torque_control.create_entities_utils import create_trajectory_switch, connect_synchronous_trajectories
from dynamic_graph.sot.torque_control.create_entities_utils import create_trajectory_generator, create_com_traj_gen, create_encoders
from dynamic_graph.sot.torque_control.create_entities_utils import create_imu_offset_compensation, create_estimators, create_imu_filter
from dynamic_graph.sot.torque_control.create_entities_utils import create_base_estimator, create_position_controller, create_torque_controller
......@@ -63,6 +64,10 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
robot.traj_gen = create_trajectory_generator(robot.device, dt);
robot.com_traj_gen = create_com_traj_gen(conf.balance_ctrl, dt);
robot.rf_force_traj_gen = create_force_traj_gen("rf_force_ref", conf.balance_ctrl.LF_FORCE_DES, dt);
robot.lf_force_traj_gen = create_force_traj_gen("rf_force_ref", conf.balance_ctrl.RF_FORCE_DES, dt);
robot.traj_sync = create_trajectory_switch();
robot.rf_traj_gen = SE3TrajectoryGenerator("tg_rf");
robot.lf_traj_gen = SE3TrajectoryGenerator("tg_lf");
robot.rf_traj_gen.init(dt);
......@@ -74,6 +79,11 @@ def main_v3(robot, startSoT=True, go_half_sitting=True, conf=None):
robot.imu_filter = create_imu_filter(robot, dt);
robot.base_estimator = create_base_estimator(robot, dt, conf.base_estimator);
connect_synchronous_trajectories(robot.traj_sync,
[robot.com_traj_gen,
robot.rf_force_traj_gen, robot.lf_force_traj_gen,
robot.rf_traj_gen, robot.lf_traj_gen])
robot.pos_ctrl = create_position_controller(robot, conf.pos_ctrl_gains, dt);
robot.torque_ctrl = create_torque_controller(robot, conf.joint_torque_controller, conf.motor_params, dt);
robot.inv_dyn = create_balance_controller(robot, conf.balance_ctrl,conf.motor_params, dt);
......
......@@ -37,7 +37,8 @@ create_topic(robot.ros, robot.device.accelerometer, 'acc');
ent.ctrl_manager.setCtrlMode('all','torque')
robot.base_estimator.set_imu_weight(0.0);
robot.com_traj_gen.playSpline("${SOTTORQUECONTROL_SOURCE_DIR}/data/traj_com_y_0.08/com_spline.curve")
robot.com_traj_gen.setSpline("/local/rbudhira/git/sot/sot-torque-control/data/traj_com_y_0.08/com_spline.curve", 1)
robot.traj_sync.turnOn()
sleep(5.0)
robot.com_traj_gen.move(1, 0.05, 1.5)
sleep(2.0*7)
......
......@@ -8,7 +8,7 @@ LF_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/lf_force.curve"
RH_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/rh_force.curve"
LH_FORCE_OUTPUT_FILE ="../data/traj_com_y_0.08/lh_force.curve"
VERIFY=False
WRITE_OUTPUT =True
WRITE_OUTPUT =False
#PLOT =True
####CONFIG######################
......
......@@ -50,10 +50,12 @@ namespace dynamicgraph
NdTrajectoryGenerator(const std::string& name)
: Entity(name)
,CONSTRUCT_SIGNAL_IN(initial_value,dynamicgraph::Vector)
,CONSTRUCT_SIGNAL(x, OUT, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_IN(trigger,bool)
,CONSTRUCT_SIGNAL(x, OUT, dynamicgraph::Vector)
,CONSTRUCT_SIGNAL_OUT(dx, dynamicgraph::Vector, m_xSOUT)
,CONSTRUCT_SIGNAL_OUT(ddx, dynamicgraph::Vector, m_xSOUT)
,m_firstIter(true)
,m_splineReady(false)
,m_initSucceeded(false)
,m_n(1)
,m_t(0)
......@@ -61,7 +63,8 @@ namespace dynamicgraph
{
BIND_SIGNAL_TO_FUNCTION(x, OUT, dynamicgraph::Vector);
Entity::signalRegistration( m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN);
Entity::signalRegistration( m_xSOUT << m_dxSOUT << m_ddxSOUT << m_initial_valueSIN
<<m_triggerSIN);
/* Commands. */
addCommand("init",
......@@ -87,10 +90,11 @@ namespace dynamicgraph
"(double) final value",
"(double) time to reach the final value in sec")));
addCommand("playSpline",
makeCommandVoid1(*this, &NdTrajectoryGenerator::playSpline,
docCommandVoid1("Load serialized spline from file",
"(string) filename")));
addCommand("setSpline",
makeCommandVoid2(*this, &NdTrajectoryGenerator::setSpline,
docCommandVoid2("Load serialized spline from file",
"(string) filename",
"(double) time to initial conf")));
/* addCommand("startTriangle",
makeCommandVoid4(*this, &NdTrajectoryGenerator::startTriangle,
......@@ -198,6 +202,7 @@ namespace dynamicgraph
}
else if(iter == m_iterLast)
{
if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
if(m_status[0]==JTG_TEXT_FILE)
{
s = (*m_textFileTrajGen)(m_t);
......@@ -215,6 +220,7 @@ namespace dynamicgraph
m_iterLast = iter;
m_t += m_dt;
if (m_triggerSIN(iter)==true && m_splineReady) startSpline();
if(m_status[0]==JTG_TEXT_FILE)
{
if(!m_textFileTrajGen->checkRange(m_t))
......@@ -241,7 +247,8 @@ namespace dynamicgraph
m_noTrajGen[i]->setInitialPoint(s(i));
m_status[i] = JTG_STOP;
}
SEND_MSG("Spline trajectory ended.", MSG_TYPE_INFO);
m_splineReady =false;
SEND_MSG("Spline trajectory ended. Remember to turn off the trigger.", MSG_TYPE_INFO);
m_t =0;
}
else
......@@ -378,7 +385,7 @@ namespace dynamicgraph
}
}
void NdTrajectoryGenerator::playSpline(const std::string& filename)
void NdTrajectoryGenerator::setSpline(const std::string& filename, const double& timeToInitConf)
{
if(!m_initSucceeded)
return SEND_MSG("Cannot start spline before initialization!",MSG_TYPE_ERROR);
......@@ -390,9 +397,10 @@ namespace dynamicgraph
if(!m_splineTrajGen->loadFromFile(filename))
return SEND_MSG("Error while loading spline"+filename, MSG_TYPE_ERROR);
SEND_MSG("Spline set to "+filename+". Now checking initial position", MSG_TYPE_INFO);
// check current configuration is not too far from initial configuration
bool needToMoveToInitConf = false;
const VectorXd& xInit = (*m_splineTrajGen)(0.002);
const VectorXd& xInit = (*m_splineTrajGen)(0.0);
assert(xInit.size() == m_n);
for(unsigned int i=0; i<m_n; i++)
if(fabs(xInit[i] - (*m_currentTrajGen[i])(m_t)[0]) > 0.001)
......@@ -410,7 +418,7 @@ namespace dynamicgraph
m_minJerkTrajGen[i]->setInitialPoint((*m_noTrajGen[i])(m_t)[0]);
m_minJerkTrajGen[i]->setFinalPoint(xInit[i]);
m_minJerkTrajGen[i]->setTimePeriod(4.0);
m_minJerkTrajGen[i]->setTimePeriod(timeToInitConf);
m_status[i] = JTG_MIN_JERK;
m_currentTrajGen[i] = m_minJerkTrajGen[i];
SEND_MSG("MinimumJerk trajectory for index "+ toString(i) +" to go to final position" + toString(xInit[i]), MSG_TYPE_WARNING);
......@@ -418,7 +426,13 @@ namespace dynamicgraph
m_t = 0.0;
return;
}
m_splineReady = true;
SEND_MSG("Spline Ready. Set trigger to true to start playing", MSG_TYPE_INFO);
}
void NdTrajectoryGenerator::startSpline()
{
if(m_status[0]==JTG_SPLINE) return;
m_t = 0.0;
for(unsigned int i=0; i<m_n; i++)
{
......@@ -577,6 +591,7 @@ namespace dynamicgraph
unsigned int i = id;
m_noTrajGen[i]->setInitialPoint((*m_currentTrajGen[i])(m_t)[0]);
m_status[i] = JTG_STOP;
m_splineReady = false;
m_currentTrajGen[i] = m_noTrajGen[i];
m_t = 0.0;
}
......
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