Exemplo n.º 1
0
void
dxJointHinge::getInfo1( dxJoint::Info1 *info )
{
    info->nub = 5;

    // see if joint is powered
    if ( limot.fmax > 0 )
        info->m = 6; // powered hinge needs an extra constraint row
    else info->m = 5;

    // if proper joint limits are specified
    // see if we're at a joint limit.
    if ( limot.lostop <= limot.histop )
    {
        dReal angle = getHingeAngle( node[0].body,
                                     node[1].body,
                                     axis1, qrel );
        // from angle, update cumulative_angle, which does not wrap
        cumulative_angle = cumulative_angle + shortest_angular_distance(cumulative_angle,angle);

        if ( limot.testRotationalLimit( cumulative_angle ) )
            info->m = 6;
    }
    // joint damping
    if ( use_damping )
      info->m = 6;
}
Exemplo n.º 2
0
dReal dJointGetHingeAngle( dJointID j )
{
    dxJointHinge* joint = ( dxJointHinge* )j;
    dAASSERT( joint );
    checktype( joint, Hinge );
    if ( joint->node[0].body )
    {
        dReal ang = getHingeAngle( joint->node[0].body,
                                   joint->node[1].body,
                                   joint->axis1,
                                   joint->qrel );
        // from angle, update cumulative_angle, which does not wrap
        joint->cumulative_angle = joint->cumulative_angle + shortest_angular_distance(joint->cumulative_angle,ang);
        if ( joint->flags & dJOINT_REVERSE )
            return -joint->cumulative_angle;
        else
            return joint->cumulative_angle;
    }
    else return 0;
}
bool FollowJointTrajectoryController::update(const ros::Time now, const ros::Duration dt)
{
    if (!initialized_)
        return false;

    /* Is trajectory active? */
    if (server_->isActive() && sampler_)
    {
        boost::mutex::scoped_lock lock(sampler_mutex_);

        /* Interpolate trajectory */
        TrajectoryPoint p = sampler_->sample(now.toSec());
        last_sample_ = p;

        /* Update joints */
        if (p.q.size() == joints_.size())
        {
            /* Position is good */
            for (size_t i = 0; i < joints_.size(); ++i)
            {
                feedback_.desired.positions[i] = p.q[i];
            }

            if (p.qd.size() == joints_.size())
            {
                /* Velocity is good */
                for (size_t i = 0; i < joints_.size(); ++i)
                {
                    feedback_.desired.velocities[i] = p.qd[i];
                }

                if (p.qdd.size() == joints_.size())
                {
                    /* Acceleration is good */
                    for (size_t i = 0; i < joints_.size(); ++i)
                    {
                        feedback_.desired.accelerations[i] = p.qdd[i];
                    }
                }
            }

            /* Fill in actual */
            for (int j = 0; j < joints_.size(); ++j)
            {
                feedback_.actual.positions[j] = joints_[j]->getPosition();
                feedback_.actual.velocities[j] = joints_[j]->getVelocity();
                feedback_.actual.effort[j] = joints_[j]->getEffort();
            }

            /* Fill in error */
            for (int j = 0; j < joints_.size(); ++j)
            {
                feedback_.error.positions[j] = shortest_angular_distance(feedback_.desired.positions[j],
                                               feedback_.actual.positions[j]);
                feedback_.error.velocities[j] = feedback_.actual.velocities[j] -
                                                feedback_.desired.velocities[j];
            }

            /* Check that we are within path tolerance */
            if (has_path_tolerance_)
            {
                for (size_t j = 0; j < joints_.size(); ++j)
                {
                    if ((path_tolerance_.q[j] > 0) &&
                            (fabs(feedback_.error.positions[j]) > path_tolerance_.q[j]))
                    {
                        control_msgs::FollowJointTrajectoryResult result;
                        result.error_code = control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
                        server_->setAborted(result, "Trajectory path tolerances violated (position).");
                        ROS_ERROR("Trajectory path tolerances violated (position).");
                    }

                    if ((path_tolerance_.qd[j] > 0) &&
                            (fabs(feedback_.error.velocities[j]) > path_tolerance_.qd[j]))
                    {
                        control_msgs::FollowJointTrajectoryResult result;
                        result.error_code = control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED;
                        server_->setAborted(result, "Trajectory path tolerances violated (velocity).");
                        ROS_ERROR("Trajectory path tolerances violated (velocity).");
                    }
                }
            }

            /* Check that we are within goal tolerance */
            if (now.toSec() >= sampler_->end_time())
            {
                bool inside_tolerances = true;
                for (size_t j = 0; j < joints_.size(); ++j)
                {
                    if ((goal_tolerance_.q[j] > 0) &&
                            (fabs(feedback_.error.positions[j]) > goal_tolerance_.q[j]))
                    {
                        inside_tolerances = false;
                    }
                }

                if (inside_tolerances)
                {
                    control_msgs::FollowJointTrajectoryResult result;
                    result.error_code = control_msgs::FollowJointTrajectoryResult::SUCCESSFUL;
                    server_->setSucceeded(result, "Trajectory succeeded.");
                    ROS_DEBUG("Trajectory succeeded");
                }
                else if (now.toSec() > (sampler_->end_time() + goal_time_tolerance_ + 0.6))  // 0.6s matches PR2
                {
                    control_msgs::FollowJointTrajectoryResult result;
                    result.error_code = control_msgs::FollowJointTrajectoryResult::GOAL_TOLERANCE_VIOLATED;
                    server_->setAborted(result, "Trajectory not executed within time limits.");
                    ROS_ERROR("Trajectory not executed within time limits");
                }
            }

            /* Update joints */
            for (size_t j = 0; j < joints_.size(); ++j)
            {
                joints_[j]->setPositionCommand(feedback_.desired.positions[j],
                                               feedback_.desired.velocities[j],
                                               0.0);
            }
            return true;
        }
    }
    else if (last_sample_.q.size() == joints_.size())
    {
        /* Hold Position */
        for (size_t j = 0; j < joints_.size(); ++j)
        {
            joints_[j]->setPositionCommand(last_sample_.q[j],
                                           0.0,
                                           0.0);
        }
        return true;
    }

    /* No goal and no sample, can't update */
    return false;
}