C++ Link::dq方法代码示例

    virtual bool control() override
    {
        joystick.readCurrentState();
        
        static const double DRIVE_GAIN = 10.0;
        static const double STEERING_P_GAIN = 3.0;
        static const double STEERING_D_GAIN = 1.0;
        static const double VEL_MAX = 20;

        double pos[2];
        pos[0] = joystick.getPosition(2);
        pos[1] = joystick.getPosition(1);

        double steer_ref = - pos[0] * 0.8;
        double q= steering->q();
        double err = steer_ref - q;
        double derr = (err - eold) / timeStep;

        steering->u() = err * STEERING_P_GAIN + derr * STEERING_D_GAIN;

        double dq = drive->dq();
        double drive_ref = -pos[1] * VEL_MAX;
        drive->u() = (drive_ref - dq) * DRIVE_GAIN;

        eold = err;
        
        return true;
    }

void AISTSimulatorItemImpl::addBody(AISTSimBody* simBody)
{
    DyBody* body = static_cast<DyBody*>(simBody->body());

    DyLink* rootLink = body->rootLink();
    rootLink->v().setZero();
    rootLink->dv().setZero();
    rootLink->w().setZero();
    rootLink->dw().setZero();
    rootLink->vo().setZero();
    rootLink->dvo().setZero();

    for(int i=0; i < body->numLinks(); ++i){
        Link* link = body->link(i);
        link->u() = 0.0;
        link->dq() = 0.0;
        link->ddq() = 0.0;
    }
    
    body->clearExternalForces();
    body->calcForwardKinematics(true, true);

    int bodyIndex;
    if(dynamicsMode.is(AISTSimulatorItem::HG_DYNAMICS)){
        ForwardDynamicsCBMPtr cbm = make_shared_aligned<ForwardDynamicsCBM>(body);
        cbm->setHighGainModeForAllJoints();
        bodyIndex = world.addBody(body, cbm);
    } else {
        bodyIndex = world.addBody(body);
    }
    bodyIndexMap[body] = bodyIndex;
}

    virtual bool control() {

        const double KP = 2000.0;
        const double KD = 5.0;

        spring->u() = -KP * spring->q() - KD * spring->dq();

        return true;
    }

bool BodyRosJointControllerItem::control()
{
  controlTime_ = controllerTarget->currentTime();
  double updateSince = controlTime_ - joint_state_last_update_;

  if (updateSince > joint_state_update_period_) {
    // publish current joint states
    joint_state_.header.stamp.fromSec(controlTime_);

    for (int i = 0; i < body()->numJoints(); i++) {
      Link* joint = body()->joint(i);
      joint_state_.position[i] = joint->q();
      joint_state_.velocity[i] = joint->dq();
      joint_state_.effort[i] = joint->u();
    }

    joint_state_publisher_.publish(joint_state_);
    joint_state_last_update_ += joint_state_update_period_;
  }

  // apply joint force based on the trajectory message
  if (has_trajectory_ && controlTime_ >= trajectory_start_) {
    if (trajectory_index_ < points_.size()) {
      unsigned int joint_size = joint_names_.size();

      for (size_t i = 0; i < joint_size; ++i) {
        std::map<std::string, int>::const_iterator it = joint_number_map_.find(joint_names_[i]);

        if (it != joint_number_map_.end()) {
          apply_message(body()->joint((*it).second), i, &points_[ trajectory_index_ ]);
        } else {
          ROS_WARN("Unknown joint name: %s", joint_names_[i].c_str());
        }
      }

      ros::Duration duration(points_[trajectory_index_].time_from_start.sec,
                             points_[trajectory_index_].time_from_start.nsec);
      trajectory_start_ += duration.toSec();
      trajectory_index_++;
    } else {
      has_trajectory_ = false;
    }
  }

  keep_attitude();

  return true;
}

void Body::initializeState()
{
    rootLink_->T() = rootLink_->Tb();

    rootLink_->v().setZero();
    rootLink_->w().setZero();
    rootLink_->dv().setZero();
    rootLink_->dw().setZero();
    
    const int n = linkTraverse_.numLinks();
    for(int i=0; i < n; ++i){
        Link* link = linkTraverse_[i];
        link->u() = 0.0;
        link->q() = 0.0;
        link->dq() = 0.0;
        link->ddq() = 0.0;
    }
 
    calcForwardKinematics(true, true);
    clearExternalForces();
    initializeDeviceStates();
}

void ODESimulatorItemImpl::addBody(ODEBody* odeBody)
{
    Body& body = *odeBody->body();

    Link* rootLink = body.rootLink();
    rootLink->v().setZero();
    rootLink->dv().setZero();
    rootLink->w().setZero();
    rootLink->dw().setZero();

    for(int i=0; i < body.numJoints(); ++i){
        Link* joint = body.joint(i);
        joint->u() = 0.0;
        joint->dq() = 0.0;
        joint->ddq() = 0.0;
    }
    
    body.clearExternalForces();
    body.calcForwardKinematics(true, true);

    odeBody->createBody(this);
}

int KinematicFaultCheckerImpl::checkFaults
(BodyItem* bodyItem, BodyMotionItem* motionItem, std::ostream& os,
 bool checkPosition, bool checkVelocity, bool checkCollision, dynamic_bitset<> linkSelection,
 double beginningTime, double endingTime)
{
    numFaults = 0;

    BodyPtr body = bodyItem->body();
    BodyMotionPtr motion = motionItem->motion();
    MultiValueSeqPtr qseq = motion->jointPosSeq();;
    MultiSE3SeqPtr pseq = motion->linkPosSeq();
    
    if((!checkPosition && !checkVelocity && !checkCollision) || body->isStaticModel() || !qseq->getNumFrames()){
        return numFaults;
    }

    BodyState orgKinematicState;
    
    if(USE_DUPLICATED_BODY){
        body = body->clone();
    } else {
        bodyItem->storeKinematicState(orgKinematicState);
    }

    CollisionDetectorPtr collisionDetector;
    WorldItem* worldItem = bodyItem->findOwnerItem<WorldItem>();
    if(worldItem){
        collisionDetector = worldItem->collisionDetector()->clone();
    } else {
        int index = CollisionDetector::factoryIndex("AISTCollisionDetector");
        if(index >= 0){
            collisionDetector = CollisionDetector::create(index);
        } else {
            collisionDetector = CollisionDetector::create(0);
            os << _("A collision detector is not found. Collisions cannot be detected this time.") << endl;
        }
    }

    addBodyToCollisionDetector(*body, *collisionDetector);
    collisionDetector->makeReady();

    const int numJoints = std::min(body->numJoints(), qseq->numParts());
    const int numLinks = std::min(body->numLinks(), pseq->numParts());

    frameRate = motion->frameRate();
    double stepRatio2 = 2.0 / frameRate;
    angleMargin = radian(angleMarginSpin.value());
    translationMargin = translationMarginSpin.value();
    velocityLimitRatio = velocityLimitRatioSpin.value() / 100.0;

    int beginningFrame = std::max(0, (int)(beginningTime * frameRate));
    int endingFrame = std::min((motion->numFrames() - 1), (int)lround(endingTime * frameRate));

    lastPosFaultFrames.clear();
    lastPosFaultFrames.resize(numJoints, std::numeric_limits<int>::min());
    lastVelFaultFrames.clear();
    lastVelFaultFrames.resize(numJoints, std::numeric_limits<int>::min());
    lastCollisionFrames.clear();

    if(checkCollision){
        Link* root = body->rootLink();
        root->p().setZero();
        root->R().setIdentity();
    }
        
    for(int frame = beginningFrame; frame <= endingFrame; ++frame){

        int prevFrame = (frame == beginningFrame) ? beginningFrame : frame - 1;
        int nextFrame = (frame == endingFrame) ? endingFrame : frame + 1;

        for(int i=0; i < numJoints; ++i){
            Link* joint = body->joint(i);
            double q = qseq->at(frame, i);
            joint->q() = q;
            if(joint->index() >= 0 && linkSelection[joint->index()]){
                if(checkPosition){
                    bool fault = false;
                    if(joint->isRotationalJoint()){
                        fault = (q > (joint->q_upper() - angleMargin) || q < (joint->q_lower() + angleMargin));
                    } else if(joint->isSlideJoint()){
                        fault = (q > (joint->q_upper() - translationMargin) || q < (joint->q_lower() + translationMargin));
                    }
                    if(fault){
                        putJointPositionFault(frame, joint, os);
                    }
                }
                if(checkVelocity){
                    double dq = (qseq->at(nextFrame, i) - qseq->at(prevFrame, i)) / stepRatio2;
                    joint->dq() = dq;
                    if(dq > (joint->dq_upper() * velocityLimitRatio) || dq < (joint->dq_lower() * velocityLimitRatio)){
                        putJointVelocityFault(frame, joint, os);
                    }
                }
            }
        }

        if(checkCollision){

            Link* link = body->link(0);
            if(!pseq->empty())
//.........这里部分代码省略.........

bool BodyRosJointControllerItem::start(Target* target)
{
  std::string topic_name;

  if (! target) {
    MessageView::instance()->putln(MessageView::ERROR, boost::format("Target not found"));
    return false;
  } else if (! target->body()) {
    MessageView::instance()->putln(MessageView::ERROR, boost::format("BodyItem not found"));
    return false;
  } else if (control_mode_name_.empty()) {
    ROS_ERROR("%s: control_mode_name_ is empty, please report to developer", __PRETTY_FUNCTION__);
    return false;
  } else {
    std::replace(control_mode_name_.begin(), control_mode_name_.end(), '-', '_');
  }

  controllerTarget = target;
  simulationBody   = target->body();
  timeStep_        = target->worldTimeStep();
  controlTime_     = target->currentTime();

  // buffer of preserve currently state of joints.
  joint_state_.header.stamp.fromSec(controlTime_);
  joint_state_.name.resize(body()->numJoints());
  joint_state_.position.resize(body()->numJoints());
  joint_state_.velocity.resize(body()->numJoints());
  joint_state_.effort.resize(body()->numJoints());

  // preserve initial state of joints.
  for (size_t i = 0; i < body()->numJoints(); i++) {
    Link* joint = body()->joint(i);

    joint_number_map_[ joint->name() ] = i;
    joint_state_.name[i]               = joint->name();
    joint_state_.position[i]           = joint->q();
    joint_state_.velocity[i]           = joint->dq();
    joint_state_.effort[i]             = joint->u();
  }

  std::string name = body()->name();
  std::replace(name.begin(), name.end(), '-', '_');

  if (hook_of_start() == false) {
    return false;
  }

  rosnode_ = boost::shared_ptr<ros::NodeHandle>(new ros::NodeHandle(name));

  topic_name                 = control_mode_name_ + "/joint_states";
  joint_state_publisher_     = rosnode_->advertise<sensor_msgs::JointState>(topic_name, 1000);
  topic_name                 = control_mode_name_ + "/set_joint_trajectory";
  joint_state_subscriber_    = rosnode_->subscribe(
                                          topic_name, 1000, &BodyRosJointControllerItem::receive_message, this);
  joint_state_update_period_ = 1.0 / joint_state_update_rate_;
  joint_state_last_update_   = controllerTarget->currentTime();

  ROS_DEBUG("Joint state update rate %f", joint_state_update_rate_);

  async_ros_spin_.reset(new ros::AsyncSpinner(0));
  async_ros_spin_->start();

  return true;
}

void LinkTraverse::calcForwardKinematics(bool calcVelocity, bool calcAcceleration) const
{
    Vector3 arm;
    int i;
    for(i=1; i <= numUpwardConnections; ++i){

        Link* link = links[i];
        const Link* child = links[i-1];

        switch(child->jointType()){

        case Link::ROTATIONAL_JOINT:
            link->R().noalias() = child->R() * AngleAxisd(child->q(), child->a()).inverse();
            arm.noalias() = link->R() * child->b();
            link->p().noalias() = child->p() - arm;

            if(calcVelocity){
                const Vector3 sw(link->R() * child->a());
                link->w() = child->w() - child->dq() * sw;
                link->v() = child->v() - link->w().cross(arm);
                
                if(calcAcceleration){
                    link->dw() = child->dw() - child->dq() * child->w().cross(sw) - (child->ddq() * sw);
                    link->dv() = child->dv() - child->w().cross(child->w().cross(arm)) - child->dw().cross(arm);
                }
            }
            break;
            
        case Link::SLIDE_JOINT:
            link->R() = child->R();
            arm.noalias() = link->R() * (child->b() + child->q() * child->d());
            link->p().noalias() = child->p() - arm;

            if(calcVelocity){
                const Vector3 sv(link->R() * child->d());
                link->w() = child->w();
                link->v().noalias() = child->v() - child->dq() * sv;

                if(calcAcceleration){
                    link->dw() = child->dw();
                    link->dv().noalias() =
                        child->dv() - child->w().cross(child->w().cross(arm)) - child->dw().cross(arm)
                        - 2.0 * child->dq() * child->w().cross(sv) - child->ddq() * sv;
                }
            }
            break;
            
        case Link::FIXED_JOINT:
        default:
            arm.noalias() = link->R() * child->b();
            link->R() = child->R();
            link->p().noalias() = child->p() - arm;

            if(calcVelocity){
                
                link->w() = child->w();
                link->v() = child->v() - link->w().cross(arm);
				
                if(calcAcceleration){
                    link->dw() = child->dw();
                    link->dv() = child->dv() - child->w().cross(child->w().cross(arm)) - child->dw().cross(arm);;
                }
            }
            break;
        }
    }

    const int n = links.size();
    for( ; i < n; ++i){
        
        Link* link = links[i];
        const Link* parent = link->parent();

        switch(link->jointType()){
            
        case Link::ROTATIONAL_JOINT:
            link->R().noalias() = parent->R() * AngleAxisd(link->q(), link->a());
            arm.noalias() = parent->R() * link->b();
            link->p().noalias() = parent->p() + arm;

            if(calcVelocity){
                const Vector3 sw(parent->R() * link->a());
                link->w().noalias() = parent->w() + sw * link->dq();
                link->v().noalias() = parent->v() + parent->w().cross(arm);

                if(calcAcceleration){
                    link->dw().noalias() = parent->dw() + link->dq() * parent->w().cross(sw) + (link->ddq() * sw);
                    link->dv().noalias() = parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm);
                }
            }
            break;
            
        case Link::SLIDE_JOINT:
            link->R() = parent->R();
            arm.noalias() = parent->R() * (link->b() + link->q() * link->d());
            link->p() = parent->p() + arm;

            if(calcVelocity){
                const Vector3 sv(parent->R() * link->d());
                link->w() = parent->w();
//.........这里部分代码省略.........