/* Set and remember power level */
void alter_power(int err, int motor_index){
int tmp_pwr = 0; // Temporary servo power value
tmp_pwr = ((mPower[motor_index] * CLICKS) + err) / CLICKS;
mPower[motor_index] = limit_range(tmp_pwr, -200, 200);
set_servo(mPower[motor_index], motor_index);
}
/*===========================================================================*
* action_pre_losttorn *
*===========================================================================*/
static void action_pre_losttorn(struct fbd_rule *rule, iovec_t *iov,
unsigned *count, size_t *size, u64_t *UNUSED(pos))
{
if (*size > rule->params.losttorn.lead)
*size = rule->params.losttorn.lead;
limit_range(iov, count, *size);
}
/*===========================================================================*
* action_pre_error *
*===========================================================================*/
static void action_pre_error(struct fbd_rule *rule, iovec_t *iov,
unsigned *count, size_t *size, u64_t *pos)
{
/* Limit the request to the part that precedes the matched range. */
*size = get_range(rule, *pos, size, NULL);
limit_range(iov, count, *size);
}
/** Run alignment algorithm.
\param first_last Last aligned position in first sequence (output)
\param second_last Last aligned position in second sequence (output)
*/
void align(int& first_last, int& second_last) const {
adjust_matrix_size();
limit_range();
make_frame();
ASSERT_TRUE(!local() || max_errors() == -1);
int& r_row = first_last;
int& r_col = second_last;
r_row = r_col = -1;
for (int row = 0; row <= max_row(); row++) {
int start_col = min_col(row);
int stop_col = max_col(row);
int min_score_col = start_col;
for (int col = start_col; col <= stop_col; col++) {
ASSERT_TRUE(col >= 0 && col < side());
ASSERT_TRUE(in(row, col));
int match = at(row - 1, col - 1) +
substitution(row, col);
int gap1 = at(row, col - 1) + gap_penalty();
int gap2 = at(row - 1, col) + gap_penalty();
int score = std::min(match, std::min(gap1, gap2));
if (local()) {
score = std::min(score, 0);
}
at(row, col) = score;
if (score < at(row, min_score_col)) {
min_score_col = col;
}
track(row, col) = (score == match) ? MATCH :
(score == gap1) ? COL_INC :
ROW_INC;
}
if (max_errors() != -1 &&
at(row, min_score_col) > max_errors()) {
break;
}
r_row = row;
r_col = min_score_col;
}
if (max_errors() == -1) {
// col -> max_col in this row
ASSERT_TRUE(in(max_row(), max_col(max_row())));
ASSERT_EQ(r_row, max_row());
r_col = max_col(max_row());
// if stopped earlier because of gap range
int last_row = contents().first_size() - 1;
int last_col = contents().second_size() - 1;
if (r_row == last_row) {
while (r_col < last_col) {
r_col += 1;
track(r_row, r_col) = COL_INC;
}
} else if (r_col == last_col) {
while (r_row < last_row) {
r_row += 1;
track(r_row, r_col) = ROW_INC;
}
} else {
throw Exception("row and column are not last");
}
}
}
void spin() {
// initialize random seed
srand(time(NULL));
// dynamics model
boost::shared_ptr<DynamicModel > dyn_model( new DynModelPlanar5() );
std::string robot_description_str;
std::string robot_semantic_description_str;
nh_.getParam("/robot_description", robot_description_str);
nh_.getParam("/robot_semantic_description", robot_semantic_description_str);
//
// collision model
//
boost::shared_ptr<self_collision::CollisionModel> col_model = self_collision::CollisionModel::parseURDF(robot_description_str);
col_model->parseSRDF(robot_semantic_description_str);
col_model->generateCollisionPairs();
// external collision objects - part of virtual link connected to the base link
self_collision::Link::VecPtrCollision col_array;
col_array.push_back( self_collision::createCollisionCapsule(0.2, 0.3, KDL::Frame(KDL::Rotation::RotX(90.0/180.0*PI), KDL::Vector(1, 0.5, 0))) );
if (!col_model->addLink("env_link", "base", col_array)) {
ROS_ERROR("ERROR: could not add external collision objects to the collision model");
return;
}
col_model->generateCollisionPairs();
//
// robot state
//
std::vector<std::string > joint_names;
joint_names.push_back("0_joint");
joint_names.push_back("1_joint");
joint_names.push_back("2_joint");
joint_names.push_back("3_joint");
joint_names.push_back("4_joint");
int ndof = joint_names.size();
Eigen::VectorXd q, dq, ddq, torque;
q.resize( ndof );
dq.resize( ndof );
ddq.resize( ndof );
torque.resize( ndof );
for (int q_idx = 0; q_idx < ndof; q_idx++) {
q[q_idx] = -0.1;
dq[q_idx] = 0.0;
ddq[q_idx] = 0.0;
torque[q_idx] = 0.0;
}
std::string effector_name = "effector";
int effector_idx = col_model->getLinkIndex(effector_name);
//
// kinematic model
//
boost::shared_ptr<KinematicModel> kin_model( new KinematicModel(robot_description_str, joint_names) );
KinematicModel::Jacobian J_r_HAND_6, J_r_HAND;
J_r_HAND_6.resize(6, ndof);
J_r_HAND.resize(3, ndof);
std::vector<KDL::Frame > links_fk(col_model->getLinksCount());
// joint limits
Eigen::VectorXd lower_limit(ndof), upper_limit(ndof), limit_range(ndof), max_trq(ndof);
int q_idx = 0;
for (std::vector<std::string >::const_iterator name_it = joint_names.begin(); name_it != joint_names.end(); name_it++, q_idx++) {
lower_limit[q_idx] = kin_model->getLowerLimit(q_idx);
upper_limit[q_idx] = kin_model->getUpperLimit(q_idx);
limit_range[q_idx] = 10.0/180.0*PI;
max_trq[q_idx] = 10.0;
}
Eigen::VectorXd max_q(ndof);
max_q(0) = 10.0/180.0*PI;
max_q(1) = 20.0/180.0*PI;
max_q(2) = 20.0/180.0*PI;
max_q(3) = 30.0/180.0*PI;
max_q(4) = 40.0/180.0*PI;
boost::shared_ptr<DynamicsSimulatorHandPose> sim( new DynamicsSimulatorHandPose(ndof, 6, effector_name, col_model, kin_model, dyn_model, joint_names, max_q) );
/*
//
// Tasks declaration
//
Task_JLC task_JLC(lower_limit, upper_limit, limit_range, max_trq);
Task_WCC task_WCC(ndof, 3, 4);
double activation_dist = 0.05;
Task_COL task_COL(ndof, activation_dist, 10.0, kin_model, col_model);
Task_HAND task_HAND(ndof, 3);
*/
/*
while (ros::ok()) {
//
// wrist collision constraint
//
Eigen::VectorXd torque_WCC(ndof);
Eigen::MatrixXd N_WCC(ndof, ndof);
task_WCC.compute(q, dq, dyn_model->getM(), dyn_model->getInvM(), torque_WCC, N_WCC, markers_pub_);
markers_pub_.publish();
ros::spinOnce();
char ch = getchar();
if (ch == 'a') {
q(3) -= 0.1;
}
else if (ch == 'd') {
q(3) += 0.1;
}
else if (ch == 's') {
q(4) -= 0.1;
}
else if (ch == 'w') {
q(4) += 0.1;
}
}
return;
*/
// loop variables
ros::Time last_time = ros::Time::now();
KDL::Frame r_HAND_target;
int loop_counter = 10000;
ros::Rate loop_rate(100);
while (true) {
generatePossiblePose(r_HAND_target, q, ndof, effector_name, col_model, kin_model);
sim->setState(q, dq, ddq);
sim->setTarget(r_HAND_target);
sim->oneStep();
if (!sim->inCollision()) {
break;
}
}
while (ros::ok()) {
if (loop_counter > 500) {
Eigen::VectorXd q_tmp(ndof);
generatePossiblePose(r_HAND_target, q_tmp, ndof, effector_name, col_model, kin_model);
sim->setTarget(r_HAND_target);
publishTransform(br, r_HAND_target, "effector_dest", "base");
loop_counter = 0;
}
loop_counter += 1;
sim->oneStep(&markers_pub_, 3000);
/* if (sim->inCollision() && !collision_in_prev_step) {
collision_in_prev_step = true;
std::cout << "collision begin" << std::endl;
}
else if (!sim->inCollision() && collision_in_prev_step) {
collision_in_prev_step = false;
std::cout << "collision end" << std::endl;
}
*/
sim->getState(q, dq, ddq);
// publish markers and robot state with limited rate
ros::Duration time_elapsed = ros::Time::now() - last_time;
if (time_elapsed.toSec() > 0.05) {
// calculate forward kinematics for all links
for (int l_idx = 0; l_idx < col_model->getLinksCount(); l_idx++) {
kin_model->calculateFk(links_fk[l_idx], col_model->getLinkName(l_idx), q);
}
publishJointState(joint_state_pub_, q, joint_names);
int m_id = 0;
m_id = addRobotModelVis(markers_pub_, m_id, col_model, links_fk);
markers_pub_.publish();
ros::Time last_time = ros::Time::now();
}
ros::spinOnce();
loop_rate.sleep();
}
}
DynamicsSimulatorHandPose::DynamicsSimulatorHandPose(int ndof, int ndim, const std::string &effector_name, const boost::shared_ptr<self_collision::CollisionModel> &col_model,
const boost::shared_ptr<KinematicModel> &kin_model,
const boost::shared_ptr<DynamicModel > &dyn_model,
const std::vector<std::string > &joint_names, const Eigen::VectorXd &q_eq, const Eigen::VectorXd &max_q,
boost::function<KDL::Twist(const KDL::Frame &x, const KDL::Frame &y)> pose_diff_function) :
ndof_(ndof),
ndim_(ndim),
effector_name_(effector_name),
q_( Eigen::VectorXd::Zero(ndof_) ),
dq_( Eigen::VectorXd::Zero(ndof_) ),
ddq_( Eigen::VectorXd::Zero(ndof_) ),
torque_( Eigen::VectorXd::Zero(ndof_) ),
col_model_(col_model),
kin_model_(kin_model),
dyn_model_(dyn_model),
links_fk_(col_model->getLinksCount()),
activation_dist_(0.05),
torque_JLC_( Eigen::VectorXd::Zero(ndof_) ),
N_JLC_( Eigen::MatrixXd::Identity(ndof_, ndof_) ),
torque_WCCr_( Eigen::VectorXd::Zero(ndof_) ),
N_WCCr_( Eigen::MatrixXd::Identity(ndof_, ndof_) ),
torque_WCCl_( Eigen::VectorXd::Zero(ndof_) ),
N_WCCl_( Eigen::MatrixXd::Identity(ndof_, ndof_) ),
torque_COL_( Eigen::VectorXd::Zero(ndof_) ),
N_COL_( Eigen::MatrixXd::Identity(ndof_, ndof_) ),
torque_HAND_( Eigen::VectorXd::Zero(ndof_) ),
N_HAND_( Eigen::MatrixXd::Identity(ndof_, ndof_) ),
torque_JOINT_( Eigen::VectorXd::Zero(ndof_) ),
N_JOINT_( Eigen::MatrixXd::Identity(ndof_, ndof_) ),
Kc_(ndim_),
Dxi_(ndim_),
Kc_JOINT_(ndof_),
Dxi_JOINT_(ndof_),
r_HAND_diff_(ndim_),
in_collision_(false),
max_q_(max_q),
q_eq_(q_eq),
pose_diff_function_(pose_diff_function)
{
for (int q_idx = 0; q_idx < ndof; q_idx++) {
Kc_JOINT_[q_idx] = 1.0;
Dxi_JOINT_[q_idx] = 0.1;
}
double Kc_lin = 200.0;
double Kc_rot = 20.0;
if (ndim_ == 3) {
Kc_[0] = Kc_lin;
Kc_[1] = Kc_lin;
Kc_[2] = Kc_rot;
}
else {
for (int dim_idx = 0; dim_idx < 3; dim_idx++) {
Kc_[dim_idx] = Kc_lin;
Kc_[dim_idx+3] = Kc_rot;
}
}
for (int dim_idx = 0; dim_idx < ndim_; dim_idx++) {
Dxi_[dim_idx] = 0.7;
}
effector_idx_ = col_model->getLinkIndex(effector_name_);
int wcc_r_q5_idx=-1, wcc_r_q6_idx=-1;
int wcc_l_q5_idx=-1, wcc_l_q6_idx=-1;
// joint limits
Eigen::VectorXd lower_limit(ndof), upper_limit(ndof), limit_range(ndof), max_trq(ndof);
int q_idx = 0;
for (std::vector<std::string >::const_iterator name_it = joint_names.begin(); name_it != joint_names.end(); name_it++, q_idx++) {
if ( (*name_it) == "right_arm_5_joint" ) {
wcc_r_q5_idx = q_idx;
}
else if ( (*name_it) == "right_arm_6_joint" ) {
wcc_r_q6_idx = q_idx;
}
else if ( (*name_it) == "left_arm_5_joint" ) {
wcc_l_q5_idx = q_idx;
}
else if ( (*name_it) == "left_arm_6_joint" ) {
wcc_l_q6_idx = q_idx;
}
lower_limit[q_idx] = kin_model->getLowerLimit(q_idx);
upper_limit[q_idx] = kin_model->getUpperLimit(q_idx);
limit_range[q_idx] = 10.0 / 180.0 * 3.141592653589793;
max_trq[q_idx] = 10.0;
}
if (ndim_ != 3 && ndim_ != 6) {
std::cout << "ERROR: DynamicsSimulatorHandPose works for 3 or 6 dimensions" << std::endl;
}
std::set<int > jlc_excluded_q_idx;
if (wcc_r_q5_idx != -1 && wcc_r_q6_idx != -1) {
jlc_excluded_q_idx.insert(wcc_r_q5_idx);
jlc_excluded_q_idx.insert(wcc_r_q6_idx);
task_WCCr_.reset( new Task_WCC(ndof, wcc_r_q5_idx, wcc_r_q6_idx, false) );
}
if (wcc_l_q5_idx != -1 && wcc_l_q6_idx != -1) {
jlc_excluded_q_idx.insert(wcc_l_q5_idx);
jlc_excluded_q_idx.insert(wcc_l_q6_idx);
task_WCCl_.reset( new Task_WCC(ndof, wcc_l_q5_idx, wcc_l_q6_idx, true) );
}
task_JLC_.reset( new Task_JLC(lower_limit, upper_limit, limit_range, max_trq, jlc_excluded_q_idx) );
task_COL_.reset( new Task_COL(ndof_, activation_dist_, 80.0, kin_model_, col_model_) );
task_HAND_.reset( new Task_HAND(ndof_, ndim_) );
J_r_HAND_6_.resize(6, ndof_);
J_r_HAND_.resize(ndim_, ndof_);
task_JOINT_.reset( new Task_JOINT(ndof_) );
}