static inline size_t get_backtrace(Frames &frame_buffer,
size_t frames_to_remove) {
void *frames[2048];
int count = ::backtrace(&frames[0], sizeof(frames) / sizeof(void *));
if (count > frames_to_remove)
frame_buffer.assign(&frames[frames_to_remove], &frames[count]);
else
frame_buffer.assign(&frames[0], &frames[count]);
while (frame_buffer.back() < (void *)1024)
frame_buffer.pop_back();
return frame_buffer.size();
}
void SslConnector::handle(AMQFrame& frame) {
bool notifyWrite = false;
{
Mutex::ScopedLock l(lock);
frames.push_back(frame);
//only ask to write if this is the end of a frameset or if we
//already have a buffers worth of data
currentSize += frame.encodedSize();
if (frame.getEof()) {
lastEof = frames.size();
notifyWrite = true;
} else {
notifyWrite = (currentSize >= maxFrameSize);
}
/*
NOTE: Moving the following line into this mutex block
is a workaround for BZ 570168, in which the test
testConcurrentSenders causes a hang about 1.5%
of the time. ( To see the hang much more frequently
leave this line out of the mutex block, and put a
small usleep just before it.)
TODO mgoulish - fix the underlying cause and then
move this call back outside the mutex.
*/
if (notifyWrite && !closed) aio->notifyPendingWrite();
}
}
int BalloonManager_br::setSingleBalloon(const char *text, uint16 x, uint16 y, uint16 winding, TextColor textColor) {
cacheAnims();
int id = _numBalloons;
Frames *src = 0;
int srcFrame = 0;
Balloon *balloon = &_intBalloons[id];
if (winding == 0) {
src = _rightBalloon;
srcFrame = 0;
} else
if (winding == 1) {
src = _leftBalloon;
srcFrame = 0;
}
assert(src);
balloon->surface = expandBalloon(src, srcFrame);
src->getRect(srcFrame, balloon->box);
_sw.write(text, 216, _textColors[textColor], balloon->surface);
// TODO: extract some text to make a name for obj
balloon->obj = _vm->_gfx->registerBalloon(new SurfaceToFrames(balloon->surface), 0);
balloon->obj->x = x + balloon->box.left;
balloon->obj->y = y + balloon->box.top;
balloon->obj->transparentKey = BALLOON_TRANSPARENT_COLOR_BR;
_numBalloons++;
return id;
}
void backtrace_log(const char *format, ...) {
const int log_fd = get_logging_fd();
if (log_fd >= 0) {
if (format && format[0]) {
va_list args;
va_start(args, format);
log(format, args);
va_end(args);
}
Frames frames;
if (get_backtrace(frames, 2))
::backtrace_symbols_fd(frames.data(), frames.size(), log_fd);
}
}
double TreeIkSolverPos_Online::CartToJnt(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
{
assert(q_out.rows() == q_in.rows());
assert(q_dot_.rows() == q_out.rows());
q_out = q_in;
// First check, if all elements in p_in are available
for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
if(frames_.find(f_des_it->first)==frames_.end())
return -2;
for (Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
{
// Get all iterators for this endpoint
Frames::iterator f_it = frames_.find(f_des_it->first);
Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);
fksolver_.JntToCart(q_out, f_it->second, f_it->first);
twist_ = diff(f_it->second, f_des_it->second);
// Checks, if the twist (twist_) exceeds the maximum translational and/or rotational velocity
// And scales them, if necessary
enforceCartVelLimits();
delta_twists_it->second = twist_;
}
double res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);
// Checks, if joint velocities (q_dot_) exceed their maximum velocities and scales them, if necessary
enforceJointVelLimits();
// Integrate
Add(q_out, q_dot_, q_out);
// Limit joint positions
for (unsigned int j = 0; j < q_min_.rows(); j++)
{
if (q_out(j) < q_min_(j))
q_out(j) = q_min_(j);
else if (q_out(j) > q_max_(j))
q_out(j) = q_max_(j);
}
return res;
}
// Called in IO thread.
size_t SslConnector::encode(char* buffer, size_t size)
{
framing::Buffer out(buffer, size);
size_t bytesWritten(0);
{
Mutex::ScopedLock l(lock);
while (!frames.empty() && out.available() >= frames.front().encodedSize() ) {
frames.front().encode(out);
QPID_LOG(trace, "SENT [" << identifier << "]: " << frames.front());
frames.pop_front();
if (lastEof) --lastEof;
}
bytesWritten = size - out.available();
currentSize -= bytesWritten;
}
if (bounds) bounds->reduce(bytesWritten);
return bytesWritten;
}
void ChannelUpdateVisitor::_updateDrawTiles( const Compound* compound,
const RenderContext& context )
{
Frames frames;
std::vector< co::ObjectVersion > frameIDs;
const Frames& outputFrames = compound->getOutputFrames();
for( FramesCIter i = outputFrames.begin(); i != outputFrames.end(); ++i)
{
Frame* frame = *i;
if( !frame->hasData( _eye )) // TODO: filter: buffers, vp, eye
continue;
frames.push_back( frame );
frameIDs.push_back( co::ObjectVersion( frame ));
}
const Channel* destChannel = compound->getInheritChannel();
const TileQueues& inputQueues = compound->getInputTileQueues();
for( TileQueuesCIter i = inputQueues.begin(); i != inputQueues.end(); ++i )
{
const TileQueue* inputQueue = *i;
const TileQueue* outputQueue = inputQueue->getOutputQueue( context.eye);
const UUID& id = outputQueue->getQueueMasterID( context.eye );
LBASSERT( id != UUID::ZERO );
ChannelFrameTilesPacket tilesPacket;
tilesPacket.isLocal = (_channel == destChannel);
tilesPacket.context = context;
tilesPacket.tasks = compound->getInheritTasks() &
( eq::fabric::TASK_CLEAR | eq::fabric::TASK_DRAW |
eq::fabric::TASK_READBACK );
tilesPacket.queueVersion.identifier = id;
tilesPacket.queueVersion.version = co::VERSION_FIRST; // eile: ???
tilesPacket.nFrames = uint32_t( frames.size( ));
_channel->send< co::ObjectVersion >( tilesPacket, frameIDs );
_updated = true;
LBLOG( LOG_TASKS ) << "TASK tiles " << _channel->getName() << " "
<< &tilesPacket << std::endl;
}
}
void backtrace_error(const char *format, ...) {
const int pid = get_interposed_pid();
if (pid >= 0) {
const int log_fd = get_logging_fd();
if (log_fd >= 0) {
log("\nerror: %s (pid=%i): ", get_process_fullpath(), pid);
if (format && format[0]) {
va_list args;
va_start(args, format);
log(format, args);
va_end(args);
}
Frames frames;
if (get_backtrace(frames, 2))
::backtrace_symbols_fd(frames.data(), frames.size(), log_fd);
}
}
}
void ChannelUpdateVisitor::_updateDrawTiles( const Compound* compound,
const RenderContext& context )
{
Frames frames;
co::ObjectVersions frameIDs;
const Frames& outputFrames = compound->getOutputFrames();
for( FramesCIter i = outputFrames.begin(); i != outputFrames.end(); ++i)
{
Frame* frame = *i;
if( !frame->hasData( _eye )) // TODO: filter: buffers, vp, eye
continue;
frames.push_back( frame );
frameIDs.push_back( co::ObjectVersion( frame ));
}
const Channel* destChannel = compound->getInheritChannel();
const TileQueues& inputQueues = compound->getInputTileQueues();
for( TileQueuesCIter i = inputQueues.begin(); i != inputQueues.end(); ++i )
{
const TileQueue* inputQueue = *i;
const TileQueue* outputQueue = inputQueue->getOutputQueue( context.eye);
const UUID& id = outputQueue->getQueueMasterID( context.eye );
LBASSERT( id != UUID::ZERO );
const bool isLocal = (_channel == destChannel);
const uint32_t tasks = compound->getInheritTasks() &
( eq::fabric::TASK_CLEAR | eq::fabric::TASK_DRAW |
eq::fabric::TASK_READBACK );
_channel->send( fabric::CMD_CHANNEL_FRAME_TILES )
<< context << isLocal << id << tasks << frameIDs;
_updated = true;
LBLOG( LOG_TASKS ) << "TASK tiles " << _channel->getName() << " "
<< std::endl;
}
}
void IECore::findSequences( const std::vector< std::string > &names, std::vector< FileSequencePtr > &sequences, size_t minSequenceSize )
{
sequences.clear();
/// this matches names of the form $prefix$frameNumber$suffix
/// placing each of those in a group of the resulting match.
/// both $prefix and $suffix may be the empty string and $frameNumber
/// may be preceded by a minus sign.
/// It also matches file extensions with 3 or 4 characters that contain numbers (for example: CR2, MP3 )
boost::regex matchExpression( std::string( "^([^#]*?)(-?[0-9]+)([^0-9#]*|[^0-9#]*\\.[a-zA-Z]{2,3}[0-9])$" ) );
/// build a mapping from ($prefix, $suffix) to a list of $frameNumbers
typedef std::vector< std::string > Frames;
typedef std::map< std::pair< std::string, std::string >, Frames > SequenceMap;
SequenceMap sequenceMap;
for ( std::vector< std::string >::const_iterator it = names.begin(); it != names.end(); ++it )
{
boost::smatch matches;
if ( boost::regex_match( *it, matches, matchExpression ) )
{
sequenceMap[
SequenceMap::key_type(
std::string( matches[1].first, matches[1].second ),
std::string( matches[3].first, matches[3].second )
)
].push_back( std::string( matches[2].first, matches[2].second ) );
}
}
for ( SequenceMap::const_iterator it = sequenceMap.begin(); it != sequenceMap.end(); ++it )
{
const SequenceMap::key_type &fixes = it->first;
const Frames &frames = it->second;
// todo: could be more efficient by writing a custom comparison function that uses indexes
// into the const Frames vector rather than duplicating the strings and sorting them directly
Frames sortedFrames = frames;
std::sort( sortedFrames.begin(), sortedFrames.end() );
/// in diabolical cases the elements of frames may not all have the same padding
/// so we'll sort them out into padded and unpadded frame sequences here, by creating
/// a map of padding->list of frames. unpadded things will be considered to have a padding
/// of 1.
typedef std::vector< FrameList::Frame > NumericFrames;
typedef std::map< unsigned int, NumericFrames > PaddingToFramesMap;
PaddingToFramesMap paddingToFrames;
for ( Frames::const_iterator fIt = sortedFrames.begin(); fIt != sortedFrames.end(); ++fIt )
{
std::string frame = *fIt;
int sign = 1;
assert( frame.size() );
if ( *frame.begin() == '-' )
{
frame = frame.substr( 1, frame.size() - 1 );
sign = -1;
}
if ( *frame.begin() == '0' || paddingToFrames.find( frame.size() ) != paddingToFrames.end() )
{
paddingToFrames[ frame.size() ].push_back( sign * boost::lexical_cast<FrameList::Frame>( frame ) );
}
else
{
paddingToFrames[ 1 ].push_back( sign * boost::lexical_cast<FrameList::Frame>( frame ) );
}
}
for ( PaddingToFramesMap::iterator pIt = paddingToFrames.begin(); pIt != paddingToFrames.end(); ++pIt )
{
const PaddingToFramesMap::key_type &padding = pIt->first;
NumericFrames &numericFrames = pIt->second;
std::sort( numericFrames.begin(), numericFrames.end() );
FrameListPtr frameList = frameListFromList( numericFrames );
std::vector< FrameList::Frame > expandedFrameList;
frameList->asList( expandedFrameList );
/// remove any sequences with less than the given minimum.
if ( expandedFrameList.size() >= minSequenceSize )
{
std::string frameTemplate;
for ( PaddingToFramesMap::key_type i = 0; i < padding; i++ )
{
frameTemplate += "#";
}
sequences.push_back(
new FileSequence(
fixes.first + frameTemplate + fixes.second,
frameList
)
);
}
}
}
}
//#include <chainidsolver_constraint_vereshchagin.hpp>
int main()
{
using namespace KDL;
//Definition of kinematic chain
//-----------------------------------------------------------------------------------------------//
//Joint (const JointType &type=None, const double &scale=1, const double &offset=0,
// const double &inertia=0, const double &damping=0, const double &stiffness=0)
Joint rotJoint0 = Joint(Joint::RotZ, 1, 0, 0.01);
Joint rotJoint1 = Joint(Joint::RotZ, 1, 0, 0.01);
//Joint rotJoint1 = Joint(Joint::None,1,0,0.01);
Frame refFrame(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, 0.0, 0.0));
Frame frame1(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
Frame frame2(Rotation::RPY(0.0, 0.0, 0.0), Vector(0.0, -0.2, 0.0));
//Segment (const Joint &joint=Joint(Joint::None),
// const Frame &f_tip=Frame::Identity(), const RigidBodyInertia &I=RigidBodyInertia::Zero())
Segment segment1 = Segment(rotJoint0, frame1);
Segment segment2 = Segment(rotJoint1, frame2);
// RotationalInertia (double Ixx=0, double Iyy=0, double Izz=0, double Ixy=0, double Ixz=0, double Iyz=0)
RotationalInertia rotInerSeg1(0.0, 0.0, 0.0, 0.0, 0.0, 0.0); //around symmetry axis of rotation
//RigidBodyInertia (double m=0, const Vector &oc=Vector::Zero(), const RotationalInertia &Ic=RotationalInertia::Zero())
RigidBodyInertia inerSegment1(1.0, Vector(0.0, -0.2, 0.0), rotInerSeg1);
RigidBodyInertia inerSegment2(1.0, Vector(0.0, -0.2, 0.0), rotInerSeg1);
segment1.setInertia(inerSegment1);
segment2.setInertia(inerSegment2);
Chain chain;
chain.addSegment(segment1);
//chain.addSegment(segment2);
//----------------------------------------------------------------------------------------------//
//Definition of constraints and external disturbances
//--------------------------------------------------------------------------------------//
JntArray arrayOfJoints(chain.getNrOfJoints());
//Constraint force matrix at the end-effector
//What is the convention for the spatial force matrix; is it the same as in thesis?
Vector constrainXLinear(0.0, 1.0, 0.0);
Vector constrainXAngular(0.0, 0.0, 0.0);
Vector constrainYLinear(0.0, 0.0, 0.0);
Vector constrainYAngular(0.0, 0.0, 0.0);
Vector constrainZLinear(0.0, 0.0, 0.0);
Vector constrainZAngular(0.0, 0.0, 0.0);
const Twist constraintForcesX(constrainXLinear, constrainXAngular);
const Twist constraintForcesY(constrainYLinear, constrainYAngular);
const Twist constraintForcesZ(constrainZLinear, constrainZAngular);
Jacobian alpha(1);
alpha.setColumn(0, constraintForcesX);
//alpha.setColumn(1,constraintForcesY);
//alpha.setColumn(2,constraintForcesZ);
//Acceleration energy at the end-effector
JntArray betha(1); //set to zero
betha(0) = 0.0;
//betha(1) = 0.0;
//betha(2) = 0.0;
//arm root acceleration
Vector linearAcc(0.0, 10, 0.0); //gravitational acceleration along Y
Vector angularAcc(0.0, 0.0, 0.0);
Twist twist1(linearAcc, angularAcc);
//external forces on the arm
Vector externalForce1(0.0, 0.0, 0.0);
Vector externalTorque1(0.0, 0.0, 0.0);
Vector externalForce2(0.0, 0.0, 0.0);
Vector externalTorque2(0.0, 0.0, 0.0);
Wrench externalNetForce1(externalForce1, externalTorque1);
Wrench externalNetForce2(externalForce2, externalTorque2);
Wrenches externalNetForce;
externalNetForce.push_back(externalNetForce1);
//externalNetForce.push_back(externalNetForce2);
//-------------------------------------------------------------------------------------//
//solvers
ChainFkSolverPos_recursive fksolver(chain);
//ChainFkSolverVel_recursive fksolverVel(chain);
int numberOfConstraints = 1;
ChainIdSolver_Vereshchagin constraintSolver(chain, twist1, numberOfConstraints);
// Initial arm position configuration/constraint
Frame frameInitialPose;
JntArray jointInitialPose(chain.getNrOfJoints());
jointInitialPose(0) = M_PI/6.0; // initial joint0 pose
//jointInitialPose(1) = M_PI/6.0; //initial joint1 pose, negative in clockwise
//cartesian space/link accelerations
Twist accLink0;
Twist accLink1;
Twists cartAcc;
cartAcc.push_back(accLink0);
//cartAcc.push_back(accLink1);
//arm joint rate configuration and initial values are zero
JntArray qDot(chain.getNrOfJoints());
//arm joint accelerations returned by the solver
JntArray qDotDot(chain.getNrOfJoints());
//arm joint torques returned by the solver
JntArray qTorque(chain.getNrOfJoints());
arrayOfJoints(0) = jointInitialPose(0);
//arrayOfJoints(1) = jointInitialPose(1);
//Calculate joint values for each cartesian position
Frame frameEEPose;
Twist frameEEVel;
double TimeConstant = 1; //Time required to complete the task move(frameinitialPose, framefinalPose) default T=10.0
double timeDelta = 0.002;
bool status;
Frame cartX1;
Frame cartX2;
Frames cartX;
cartX.push_back(cartX1);
cartX.push_back(cartX2);
Twists cartXDot;
cartXDot.push_back(accLink0);
cartXDot.push_back(accLink1);
Twists cartXDotDot;
cartXDotDot.push_back(accLink0);
cartXDotDot.push_back(accLink1);
for (double t = 0.0; t <= 5*TimeConstant; t = t + timeDelta)
{
//at time t0, inputs are arrayOfjoints(0,pi/2.0), qdot(0,0)
//qDot(0) = 0.5;
// qDot(1) = 0.5;
//printf("Inside the main loop %f %f\n",qTorque(0), qTorque(1));
//NOTE AZAMAT:what is qTorque? in implementation returned qTorque is constraint torque generated by the virtual constraint forces.
// these torques are required to satify a virtual constraint and basically are real torques applied to joints to achieve
// these constraint. But if additionally to the constraint generated torques we also had internal motor torques, then should not they
//be added together?
//qTorque(1) = qTorque(1)+internalJointTorque1;
// In what frame of reference are the results expressed?
status = constraintSolver.CartToJnt(arrayOfJoints, qDot, qDotDot, alpha, betha, externalNetForce, qTorque);
if (status >= 0)
{
//For 2-dof arm
//printf("%f %f %f %f %f %f %f %f %f %f %f\n",t , arrayOfJoints(0), arrayOfJoints(1), qDot(0), qDot(1), qDotDot(0), qDotDot(1), qTorque(0), qTorque(1), frameEEPose.p.x(), frameEEPose.p.y());
//printf("%f %f %f %f %f %f %f\n", t, cartAcc[0].vel.x(), cartAcc[0].vel.y(), cartAcc[0].rot.z(), cartAcc[1].vel.x(), cartAcc[1].vel.y(), cartAcc[1].rot.z());
//For 1-dof
printf("Joint actual %f %f %f %f %f\n", t, arrayOfJoints(0), qDot(0), qDotDot(0), qTorque(0));
std::cout<<std::endl;
}
constraintSolver.getLinkCartesianPose(cartX);
constraintSolver.getLinkCartesianVelocity(cartXDot);
constraintSolver.getLinkCartesianAcceleration(cartXDotDot);
constraintSolver.getLinkAcceleration(cartAcc);
printf("Cartesian acc local %f %f %f %f\n", t, cartAcc[0].vel.x(), cartAcc[0].vel.y(), cartAcc[0].rot.z());
printf("Cartesian actual %f %f %f %f %f\n", t, cartX[0].p.x(), cartX[0].p.y(), cartXDot[0].vel[0], cartXDot[0].vel[1]);
std::cout<<std::endl;
//printf("External torque %f\n", externalNetForce[0].torque.z());
//Integration at the given "instanteneous" interval for joint position and velocity.
//These will be inputs for the next time instant
//actually here it should be a time interval and not time from the beginning, that is why timeDelta;
arrayOfJoints(0) = arrayOfJoints(0) + (qDot(0) + qDotDot(0) * timeDelta / 2.0) * timeDelta;
qDot(0) = qDot(0) + qDotDot(0) * timeDelta;
//arrayOfJoints(1) = arrayOfJoints(1) + (qDot(1) + qDotDot(1)*timeDelta/2.0)*timeDelta;
//qDot(1) = qDot(1)+qDotDot(1)*timeDelta;
//For cartesian space control one needs to calculate from the obtained joint space value, new cartesian space poses.
//Then based on the difference of the signal (desired-actual) we define a regulation function (controller)
// this difference should be compensated either by constraint forces or by joint torques.
// The current version of the algorithm assumes that we don't have control over torques, thus change in constraint forces
//should be a valid option.
}
//torque1=[(m1+m2)a1^2+m2a2^2+2m2a1a2cos2]qDotDot1+[m2a2^2+m2a1a2cos2]qDotDot2-m2a1a2(2qDot1qDot2+qDot2^2)sin2+(m1+m2)ga1cos2+m2ga2cos(1+2)
//torque2=[m2a2^2+m2a1a2cos2]qDotDot1+m2a2^2qDotDot2+m2a1a2qDot1^2sin2+m2ga2cos(1+2)
return 0;
}
void KTLayer::setFrames(const Frames &frames)
{
m_frames = frames;
m_framesCount = frames.count();
}
size_t size() { return frames.size(); }
double TreeIkSolverPos_Online::CartToJnt_it(const JntArray& q_in, const Frames& p_in, JntArray& q_out)
{
assert(q_out.rows() == q_in.rows());
assert(q_out_old_.rows() == q_in.rows());
assert(q_dot_.rows() == q_in.rows());
assert(q_dot_old_.rows() == q_in.rows());
assert(q_dot_new_.rows() == q_in.rows());
q_out = q_in;
SetToZero(q_dot_);
SetToZero(q_dot_old_);
SetToZero(q_dot_new_);
twist_ = Twist::Zero();
// First check, if all elements in p_in are available
unsigned int nr_of_endeffectors = 0;
nr_of_still_endeffectors_ = 0;
low_pass_adj_factor_ = 0.0;
for(Frames::const_iterator f_des_it=p_in.begin();f_des_it!=p_in.end();++f_des_it)
{
if(frames_.find(f_des_it->first)==frames_.end())
return -2;
else
{
/*
Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
old_twists_it->second = Twist::Zero();
Frames::iterator f_0_it = frames_0_.find(f_des_it->first);
*/
/*
Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
old_twists_it->second = diff(f_old_it->second, f_des_it->second);
if (){};
*/
Frames::iterator f_old_it = p_in_old_.find(f_des_it->first);
Frames::iterator f_des_pos_l_it = frames_pos_lim_.find(f_des_it->first);
twist_ = diff(f_old_it->second, f_des_it->second);
/*
if(sqrt(pow(twist_.vel.x(), 2) + pow(twist_.vel.y(), 2) + pow(twist_.vel.z(), 2)) < x_dot_trans_min_)
{
f_des_pos_l_it->second.p = addDelta(f_old_it->second.p, (0.1 * x_dot_trans_max_ * twist_.vel));
std::cout << "old position is used." << std::endl;
}
else
f_des_pos_l_it->second.p = f_des_it->second.p;
if(sqrt(pow(twist_.rot.x(), 2) + pow(twist_.rot.y(), 2) + pow(twist_.rot.z(), 2)) < x_dot_rot_min_)
{
f_des_pos_l_it->second.M = addDelta(f_old_it->second.M, (0.1 * x_dot_rot_max_ * twist_.rot));
std::cout << "old orientation is used." << std::endl;
}
else
f_des_pos_l_it->second.M = f_des_it->second.M;
*/
f_des_pos_l_it->second.p = f_des_it->second.p;
f_des_pos_l_it->second.M = f_des_it->second.M;
Frames::iterator f_des_vel_l_it = frames_vel_lim_.find(f_des_it->first);
fksolver_.JntToCart(q_in, f_des_vel_l_it->second, f_des_it->first);
twist_ = diff(f_des_vel_l_it->second, f_des_pos_l_it->second);
f_des_vel_l_it->second = addDelta(f_des_vel_l_it->second, twist_);
nr_of_endeffectors++;
}
}
if(nr_of_still_endeffectors_ == nr_of_endeffectors)
{
small_task_space_movement_ = true;
}
else
small_task_space_movement_ = false;
unsigned int k=0;
double res = 0.0;
while(++k <= maxiter_)
{
//for (Frames::const_iterator f_des_it=p_in.begin(); f_des_it!=p_in.end(); ++f_des_it)
for (Frames::const_iterator f_des_it=frames_vel_lim_.begin(); f_des_it!=frames_vel_lim_.end(); ++f_des_it)
{
// Get all iterators for this endpoint
Frames::iterator f_it = frames_.find(f_des_it->first);
Twists::iterator delta_twists_it = delta_twists_.find(f_des_it->first);
Twists::iterator old_twists_it = old_twists_.find(f_des_it->first);
Frames::iterator f_0_it = frames_vel_lim_.find(f_des_it->first);
fksolver_.JntToCart(q_out, f_it->second, f_it->first);
// Checks, if the current overall twist exceeds the maximum translational and/or rotational velocity.
// If so, the velocities of the overall twist get scaled and a new current twist is calculated.
delta_twists_it->second = diff(f_it->second, f_des_it->second);
old_twists_it->second = diff(f_0_it->second, f_it->second);
enforceCartVelLimits_it(old_twists_it->second, delta_twists_it->second);
}
res = iksolver_.CartToJnt(q_out, delta_twists_, q_dot_);
if (res < eps_)
{
break;
//return res;
}
// Checks, if joint velocities (q_dot_) exceed their maximum velocities and scales them, if necessary
//Subtract(q_out, q_in, q_dot_old_);
//enforceJointVelLimits_it(q_dot_old_, q_dot_);
Subtract(q_out, q_in, q_dot_old_);
Add(q_dot_old_, q_dot_, q_dot_);
enforceJointVelLimits();
Subtract(q_dot_, q_dot_old_, q_dot_);
// Integrate
Add(q_out, q_dot_, q_out);
// Limit joint positions
for (unsigned int j = 0; j < q_min_.rows(); ++j)
{
if (q_out(j) < q_min_(j))
q_out(j) = q_min_(j);
else if (q_out(j) > q_max_(j))
q_out(j) = q_max_(j);
}
}
Subtract(q_out, q_in, q_dot_);
Add(q_in, q_dot_, q_out);
filter(q_dot_, q_out, q_out_old_);
q_out_old_ = q_out;
p_in_old_ = p_in;
if (k <= maxiter_)
return res;
else
return -3;
}
