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())
{
const SE3& p = pseq->at(frame, 0);
link->p() = p.translation();
link->R() = p.rotation().toRotationMatrix();
}
else
{
link->p() = Vector3d(0., 0., 0.);
link->R() = Matrix3d::Identity();
}
body->calcForwardKinematics();
for(int i=1; i < numLinks; ++i){
link = body->link(i);
if(!pseq->empty())
{
const SE3& p = pseq->at(frame, i);
link->p() = p.translation();
link->R() = p.rotation().toRotationMatrix();
}
}
for(int i=0; i < numLinks; ++i){
link = body->link(i);
collisionDetector->updatePosition(i, link->position());
}
collisionDetector->detectCollisions(
boost::bind(&KinematicFaultCheckerImpl::putSelfCollision, this, body.get(), frame, _1, boost::ref(os)));
}
}
if(!USE_DUPLICATED_BODY){
bodyItem->restoreKinematicState(orgKinematicState);
}
return numFaults;
}
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();
link->v().noalias() = parent->v() + sv * link->dq();
if(calcAcceleration){
link->dw() = parent->dw();
link->dv().noalias() = parent->dv() + parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm)
+ 2.0 * link->dq() * parent->w().cross(sv) + link->ddq() * sv;
}
}
break;
case Link::FIXED_JOINT:
default:
arm.noalias() = parent->R() * link->b();
link->R() = parent->R();
link->p().noalias() = arm + parent->p();
if(calcVelocity){
link->w() = parent->w();
link->v() = parent->v() + parent->w().cross(arm);;
if(calcAcceleration){
link->dw() = parent->dw();
link->dv().noalias() = parent->dv() +
parent->w().cross(parent->w().cross(arm)) + parent->dw().cross(arm);
}
}
break;
}
}
}
