예제 #1
0
 /**
  * Makes sure that all jointstate are within their respective limits
  *
  * Will throw if this is not the case. Will also throw if there are no
  * limits for a particular joint.
  */
 void validate( const base::samples::Joints& joints ) const
 {
     if (joints.hasNames())
     {
         for( size_t i=0; i<joints.size(); i++ )
             (*this)[joints.names[i]].validate( joints[i] );
     }
     else
     {
         for( size_t i=0; i<joints.size(); i++ )
             (*this)[i].validate( joints[i] );
     }
 }
예제 #2
0
bool Lateral::compute(const base::commands::Motion2D &motionCmd, base::samples::Joints& actuatorsCommand)
{   
    double speed = translateSpeedToWheelSpeed(motionCmd.translation);
   
    for (auto jointActuator: controllerBase->getJointActuators())
    {
        JointCmd* positionCmd = jointActuator->getJointCmdForType(JointCmdType::Position);
        JointCmd* steeringCmd = jointActuator->getJointCmdForType(JointCmdType::Speed);

        if (!positionCmd || !steeringCmd)
        {
            continue;
        }
        
        base::JointState &positionJointState(actuatorsCommand[actuatorsCommand.mapNameToIndex(positionCmd->getName())]);
        base::JointState &steeringJointState(actuatorsCommand[actuatorsCommand.mapNameToIndex(steeringCmd->getName())]);
        

        positionJointState.position = motionCmd.heading.getRad();
        steeringJointState.speed = speed;
        if(motionCmd.heading.getRad() > M_PI){
            positionJointState.position -= M_PI;
            positionJointState.speed *= -1;
        }
    }

    return true;
}
예제 #3
0
 bool isValid( const base::samples::Joints& joints ) const
 {
     if (joints.hasNames())
     {
         for( size_t i=0; i<joints.size(); i++ )
             if (! (*this)[joints.names[i]].isValid( joints[i] ))
                 return false;
     }
     else
     {
         for( size_t i=0; i<joints.size(); i++ )
             if (! (*this)[i].isValid( joints[i] ))
                 return false;
     }
     return true;
 }
void Task::thruster_samplesCallback(const base::Time& ts, const base::samples::Joints& status)
{  
  //base::Time temp = base::Time::now();
  if(last_speed_time.isNull() || ts.toSeconds() - last_speed_time.toSeconds() > _speed_samples_timeout.get() ){
  
    
    base::samples::Joints j = status;
    last_motion = ts; 

    //If we have no 6 thruster, fill up the joints with zeros
    /*if(j.size() < 6){
      
      unsigned int size = j.size();
      j.elements.resize(6);
      j.names.resize(6);
      
      for(; size < 6; size++){
        
        j.elements[size].raw = 0.0;
        
      }
      
    }*/
    
    if(status.hasNames()){
    
      for(unsigned int i = 0; i < status.size() && i < config.joint_names.size(); i++){
        
          try{
            j.elements[i] = status[config.joint_names[i]]; 
          }
          catch(...){
          }	
      }  
    }

    if(orientation_sample_recieved){
      localizer->update_dead_reckoning(j);
      localizer->update(j, *map);
      
    }else{
      changeState(NO_ORIENTATION);
    }
  }
  //std::cout << "Calc time thruster samples: " << base::Time::now().toSeconds() - temp.toSeconds() << std::endl;  
}
예제 #5
0
void TransformationCalculator::update(const base::samples::Joints& joints)
{
    base::JointState j_state;
    std::string  j_name;
    std::string  seg_name;
    KDL::Segment seg;
    KDL::Frame kdl_pose;
    std::map<std::string,base::samples::RigidBodyState>::iterator transform_it;

    if(!is_initialized_)
        throw std::runtime_error("TransformationCalculator was not initialized. Call load_robot_model first");

    for(uint i=0; i<joints.size(); i++){
        j_state = joints.elements[i];
        j_name = joints.names[i];

        if(is_invalid(j_state.position)){
            LOG_ERROR("Received invalid joint position for joint %s", j_name.c_str());
            continue;
        }

        try{
            //Get segment
            seg_name = joint_name2seg_name_[j_name];
            seg = get_segment_by_segment_name(seg_name);

            //Calculate pose
            kdl_pose = seg.pose(j_state.position);

            //Convert transform to eigen
            transform_it = moving_joints_transforms_.find(j_name);
            if(transform_it == moving_joints_transforms_.end())
                throw("Unknown frame name. Should be thrown at this point, but ealier in get_segment.");

            convert(kdl_pose, transform_it->second);

            //Set timestap
            transform_it->second.time = joints.time;
        }
        catch(...){
            LOG_WARN("Joint %s or segment %s are unknown", j_name.c_str(), seg_name.c_str());
            continue;
        }
    }
}
void RobotVisualization::setJointsState(const base::samples::Joints &sample){
    vector<string> names = getJointNames();
    if (sample.hasNames())
        names = sample.names;
    if (names.size() != sample.elements.size())
        throw std::runtime_error("RobotVisualization::updateDataIntern: state vector size and expected joint size differ, and there are no names in the Joints sample");

    for(uint i=0; i<names.size(); i++){
        if(base::isUnknown(sample[i].position) || base::isInfinity(sample[i].position)){
            if(names.size()){
                LOG_ERROR("Position of joint %s is invalid: %d", sample.names[i].c_str(), sample[i].position);
            }
            else{
                LOG_ERROR("Position of joint %d is invalid: %d", i, sample[i].position);
            }
            //throw std::runtime_error("RobotVisualization::updateDataIntern: invalid joint position detected.");
            setJointState(names[i], 0);
        }
        else{
            setJointState(names[i], sample[i].position);
        }
    }
}