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;
}
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;
}
