void config_t::global_to_relative( const config_t& root_config )
{
//Have an identity temporary orientaiton
util::quaternion_t tmp_orient;
tmp_orient.zero();
//Then, set our position to be the relative position
this->set_position( this->get_position() - root_config.get_position() );
tmp_orient /= root_config.get_orientation();
this->set_position(vector_t(tmp_orient.qv_rotation(this->get_position())));
tmp_orient *= this->get_orientation();
tmp_orient.normalize();
PRX_ASSERT(tmp_orient.is_valid());
this->set_orientation(tmp_orient);
}
bool unigripper_motoman_plant_t::IK_solver(const config_t& effector_config, space_point_t* computed_state, bool set_grasping, const space_point_t* seed_state, bool do_min)
{
double qx,qy,qz,qw;
double x,y,z;
effector_config.get_orientation().get(qx,qy,qz,qw);
effector_config.get_position(x,y,z);
KDL::Chain chain1;
my_tree->getChain("base_link","vacuum_volume",chain1);
KDL::JntArray q(chain1.getNrOfJoints());
KDL::JntArray q_out(chain1.getNrOfJoints());
KDL::JntArray q_min(chain1.getNrOfJoints());
KDL::JntArray q_max(chain1.getNrOfJoints());
std::vector< double > state_var;
if( seed_state != NULL )
state_space->copy_point_to_vector( seed_state, state_var );
else
state_space->copy_to_vector( state_var );
q(0) = state_var[0];
q(1) = state_var[1];
q(2) = state_var[2];
q(3) = state_var[3];
q(4) = state_var[4];
q(5) = state_var[5];
q(6) = state_var[6];
q(7) = state_var[7];
q(8) = state_var[15];
q_min(0) = state_space->get_bounds()[0]->get_lower_bound();
q_min(1) = state_space->get_bounds()[1]->get_lower_bound();
q_min(2) = state_space->get_bounds()[2]->get_lower_bound();
q_min(3) = state_space->get_bounds()[3]->get_lower_bound();
q_min(4) = state_space->get_bounds()[4]->get_lower_bound();
q_min(5) = state_space->get_bounds()[5]->get_lower_bound();
q_min(6) = state_space->get_bounds()[6]->get_lower_bound();
q_min(7) = state_space->get_bounds()[7]->get_lower_bound();
q_min(8) = state_space->get_bounds()[15]->get_lower_bound();
q_max(0) = state_space->get_bounds()[0]->get_upper_bound();
q_max(1) = state_space->get_bounds()[1]->get_upper_bound();
q_max(2) = state_space->get_bounds()[2]->get_upper_bound();
q_max(3) = state_space->get_bounds()[3]->get_upper_bound();
q_max(4) = state_space->get_bounds()[4]->get_upper_bound();
q_max(5) = state_space->get_bounds()[5]->get_upper_bound();
q_max(6) = state_space->get_bounds()[6]->get_upper_bound();
q_max(7) = state_space->get_bounds()[7]->get_upper_bound();
q_max(8) = state_space->get_bounds()[15]->get_upper_bound();
KDL::ChainFkSolverPos_recursive fk_solver(chain1);
KDL::ChainIkSolverVel_pinv ik_solver_vel(chain1);
KDL::ChainIkSolverPos_NR_JL ik_solver(chain1,q_min,q_max,fk_solver,ik_solver_vel,1000,1e-6);
KDL::Frame F(KDL::Rotation::Quaternion(qx,qy,qz,qw),KDL::Vector(x,y,z));
bool ik_res = (ik_solver.CartToJnt(q,F,q_out)>=0);
if(ik_res)
{
std::vector<double> state_vec;
state_vec = state_var;
state_vec[0] = q_out(0);
state_vec[1] = q_out(1);
state_vec[2] = q_out(2);
state_vec[3] = q_out(3);
state_vec[4] = q_out(4);
state_vec[5] = q_out(5);
state_vec[6] = q_out(6);
state_vec[7] = q_out(7);
state_vec[15] = q_out(8);
state_vec[16] = set_grasping ? (double)GRIPPER_CLOSED : GRIPPER_OPEN;
state_space->copy_vector_to_point( state_vec, computed_state );
}
return ik_res;
}
void config_t::relative_to_global(const config_t& root_config)
{
set_position(vector_t(root_config.get_orientation().qv_rotation(position)));
*this = root_config + *this;
}
void config_t::interpolate(const config_t& start, const config_t& target, double t)
{
position.interpolate(start.get_position(), target.get_position(), t);
orientation.interpolate(start.get_orientation(), target.get_orientation(), t);
}
