Skip to content

Commit

Permalink
fixed connection loss -> merged call. traj. controller with TG
Browse files Browse the repository at this point in the history
  • Loading branch information
nhuebel committed Sep 5, 2013
1 parent 2f5f4a6 commit 8bc3242
Show file tree
Hide file tree
Showing 4 changed files with 64 additions and 19 deletions.
32 changes: 29 additions & 3 deletions lwr_fri/src/FRIComponent.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -257,17 +257,43 @@ void FRIComponent::updateHook() {
if (m_control_mode == 1) {
m_cmd_data.cmd.cmdFlags = FRI_CMD_JNTPOS;
if(updateGenerator()){
if (NewData == m_jntPosPort.read(m_jntPos))
for (unsigned int i = 0; i < LBR_MNJ; i++)
if (NewData == m_jntPosPort.read(m_jntPos)){
last_cmd_jnt_pos.clear();
last_cmd_jnt_pos.reserve(LBR_MNJ);
for (unsigned int i = 0; i < LBR_MNJ; i++){
m_cmd_data.cmd.jntPos[i] = m_jntPos[i];
last_cmd_jnt_pos.push_back(m_jntPos[i]);
}
// std::cout << "FRI: "<< m_jntPos[0] << " " << m_jntPos[1] << " " << m_jntPos[2] << " "
// << m_jntPos[3] << " " << m_jntPos[4] << " " << m_jntPos[5] << " "
// << m_jntPos[6] << std::endl;
}else{
//std::cout << "NO new data" << std::endl;
}
}else{
//std::cout << "sending measurements" << std::endl;
for (unsigned int i = 0; i < LBR_MNJ; i++)
m_cmd_data.cmd.jntPos[i] = m_msr_data.data.msrJntPos[i];
}

/*
}else{
std::cout << "updateGenerator() returned false" << std::endl;
if (last_cmd_jnt_pos.size() == 0) {
std::cout << "sending measurements" << std::endl;
for (unsigned int i = 0; i < LBR_MNJ; i++)
m_cmd_data.cmd.jntPos[i] = m_msr_data.data.msrJntPos[i];
} else {
std::cout << "sending last cmd data" << std::endl;
for (unsigned int i = 0; i < LBR_MNJ; i++)
m_cmd_data.cmd.jntPos[i] = last_cmd_jnt_pos[i];
}
}
std::cout << "FRI cmd: " << m_cmd_data.cmd.jntPos[0]*57.2957795 << " " << m_cmd_data.cmd.jntPos[1]*57.2957795 << " "
<< m_cmd_data.cmd.jntPos[2]*57.2957795 << " " << m_cmd_data.cmd.jntPos[3]*57.2957795 << " "
<< m_cmd_data.cmd.jntPos[4]*57.2957795 << " " << m_cmd_data.cmd.jntPos[5]*57.2957795 << " "
<< m_cmd_data.cmd.jntPos[6]*57.2957795 << std::endl;
*/

} else if (m_control_mode == 2) {
m_cmd_data.cmd.cmdFlags = FRI_CMD_JNTPOS;
Expand Down
3 changes: 3 additions & 0 deletions lwr_fri/src/FRIComponent.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -113,6 +113,9 @@ class FRIComponent: public RTT::TaskContext {
RTT::os::TimeService::ticks time_begin;
RTT::os::TimeService::Seconds time_passed;

//store last cmd joint positions
std::vector<fri_float_t> last_cmd_jnt_pos;

RTT::OperationCaller<bool(void)> updateGenerator;
// RTT::OperationCaller<bool(void)> updateCG;
};
Expand Down
47 changes: 32 additions & 15 deletions trajectory_generator/src/TrajectoryGenerator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ namespace trajectory_generator

timeLogger.open("timeLog.txt");


state=time_opt;

//addition from call. traj. contrl.
this->ports()->addEventPort("newTrajectoryFromROS", iprt_trajectory,
Expand Down Expand Up @@ -123,7 +123,7 @@ namespace trajectory_generator
if (abs(robotState.velocity[i]) > abs(vM[i]))
{
scale[i] = robotState.velocity[i]/vM[i];
cout << "The requested velocity is higher than it should. Rescaling from "
cout << "[TrajectoryGenerator::jntVelScaling]: The requested velocity is higher than it should. Rescaling from "
<< robotState.velocity[i] << " to " << vM[i] << " by a factor " << scale[i] << endl;
}
}
Expand Down Expand Up @@ -194,9 +194,9 @@ namespace trajectory_generator
{
if(doSync==true && addFinalVel==true){
doSync = false;
cout << "Sync with non zero final velocities is not yet implemented" << endl;
cout << "[TrajectoryGenerator::startHook]: Sync with non zero final velocities is not yet implemented" << endl;
}
std::cout << "TrajectoryGenerator::Trajectory generator running" << std::endl;
std::cout << "[TrajectoryGenerator::startHook]: Trajectory generator running" << std::endl;

return true;
}
Expand All @@ -205,7 +205,7 @@ namespace trajectory_generator

bool TrajectoryGenerator::generateNewVelocityProfilesJntPosInput(RTT::base::PortInterface* portInterface)
{
state = time_opt;
cout << "[TrajectoryGenerator::generateNewVelocityProfilesJntPosInput]: New jnt pos received." << endl;

//Create joint specific velocity profiles
maxDuration = 0.0;
Expand All @@ -223,11 +223,10 @@ namespace trajectory_generator
lastCommandedPoseJntPos = cmdJntState.position;
lastCommandedPoseJntVel = cmdJntState.velocity;


for(int i=0; i < 7; i++){
if(lastCommandedPoseJntPos[i]<p_min[i] || lastCommandedPoseJntPos[i]>p_max[i]){
//log(Info) << "Commanded joint position out of bounds" << endlog();
cout << "Commanded joint position out of bounds " << lastCommandedPoseJntPos[i] << endl;
cout << "[TrajectoryGenerator::generateNewVelocityProfilesJntPosInput]: Commanded joint position out of bounds " << lastCommandedPoseJntPos[i] << endl;
return false;
}

Expand All @@ -237,7 +236,7 @@ namespace trajectory_generator
// Then, if the motion due to deceleration/acceleration needed to stop_the_robot/reach_the_final_vel
// is higher than the distance that we have to the position limits, final state cannot be achieved
if (p_aux < 0.5*lastCommandedPoseJntVel[i]*lastCommandedPoseJntVel[i]/a_max[i]){
cout << "Commanded final velocity out of bounds " << lastCommandedPoseJntVel[i] << endl;
cout << "[TrajectoryGenerator::generateNewVelocityProfilesJntPosInput]: Commanded final velocity out of bounds " << lastCommandedPoseJntVel[i] << endl;
return false;
}
}
Expand Down Expand Up @@ -281,6 +280,15 @@ namespace trajectory_generator

//Set times
time_begin = os::TimeService::Instance()->getTicks();

//Switch to time-opt state
state = time_opt;
cout << "[TrajectoryGenerator::generateNewVelocityProfilesJntPosInput]: Switched to time-opt state." << endl;

// reset list of points for iterating state
this->trajectory.points.clear();
this->trajectory_iterator = this->trajectory.points.begin();

#if DEBUG
cout << "A new set of motion profiles were successfully created." << endl;
#endif
Expand Down Expand Up @@ -309,6 +317,7 @@ namespace trajectory_generator
<< jntPosCmd[3] << " " << jntPosCmd[4] << " " << jntPosCmd[5] << " "
<< jntPosCmd[6] << endlog();
#endif

output_jntPosPort.write(jntPosCmd);
return true;
}else{
Expand All @@ -322,6 +331,11 @@ namespace trajectory_generator
else
return false;
}
else
{
ROS_WARN("[TrajectoryGenerator::updateTG]: Undefined state!");
return false;
}
}


Expand All @@ -343,27 +357,30 @@ namespace trajectory_generator

void TrajectoryGenerator::evNewTrajectory(RTT::base::PortInterface* portInterface)
{
state = iterating;

if (iprt_trajectory.read(this->trajectory) == RTT::NewData)
{
//std::cout << "New trajectory received!" << std::endl;

this->trajectory_iterator = this->trajectory.points.begin();

//std::cout << "Start drawing" << std::endl;
state = iterating;
motionProfile.clear(); //make sure motion profile of time-opt is stopped
std::cout << "[TrajectoryGenerator::evNewTrajectory]: New trajectory received! Switched to iterating state." << std::endl;

}
else
std::cout << "ERROR: no new trajectory" << std::endl;
std::cout << "[TrajectoryGenerator::evNewTrajectory]: ERROR: no new trajectory" << std::endl;
}


bool TrajectoryGenerator::getNextPointOnCalligraphyTrajectory()
{

if (this->trajectory.points.size() == 0)
return false;

{
ROS_WARN("[TrajectoryGenerator::getNextPointOnCalligraphyTrajectory]: Trajectory empty");
return false;
}
// the last point in the trajectory has been sent...
if (this->trajectory_iterator == this->trajectory.points.end())
{
Expand All @@ -373,7 +390,7 @@ namespace trajectory_generator
finished.data = true;
oprt_character_done.write(finished);

//std::cout << "Drawing finished" << std::endl;
ROS_INFO("[TrajectoryGenerator::getNextPointOnCalligraphyTrajectory]: Trajectory finished");

// reset trajectory
this->trajectory.points.clear();
Expand Down
1 change: 0 additions & 1 deletion trajectory_generator/src/TrajectoryGenerator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,6 @@ namespace trajectory_generator
/**
* \brief state in which the TG is in [time-opt.|iterating received vector]
*/
//no need for initialization since both possible callback fcns will define it
enum state_t { time_opt, iterating };
state_t state;

Expand Down

0 comments on commit 8bc3242

Please sign in to comment.