bool unigripper_motoman_plant_t::IK_steering( const config_t& start_config, const config_t& goal_config, sim::plan_t& result_plan, bool set_grasping ) { //Begin by determining a distance between the start and goal configurations double distance = start_config.get_position().distance( goal_config.get_position() ); //Set the number of interpolation steps double steps = std::ceil(distance / MAX_IK_STEP); //Clear out the plan we were given, we will be filling it in ourselves result_plan.clear(); //Keep around an intermediate plan for systematically building up the solution plan_t intermediate_plan = result_plan; //Store the arm's current state as the previous state state_space->copy_to_point(prev_st); //So first, let's do the IK for the start configuration if( !IK_solver( start_config, prev_st, set_grasping, prev_st, true ) ) { PRX_DEBUG_COLOR("IK failed in Steering for the start config.", PRX_TEXT_RED); return false; } //Storage for the interpolated configuration config_t interpolated_config; //Then, for each step for( double i=1; i<=steps; ++i ) { //Compute the next interpolated configuration double t = PRX_MINIMUM( (i/steps), 1.0 ); interpolated_config.interpolate( start_config, goal_config, t ); //Compute Minimum IK for interpolated configuration (Pose, seed state, soln) if( IK_solver( interpolated_config, inter_st, set_grasping, prev_st, true ) ) { //Create a plan by steering between states and add it to the resulting plan intermediate_plan.clear(); steering_function(prev_st, inter_st, intermediate_plan); result_plan += intermediate_plan; //And make sure we store this state as our prior state state_space->copy_point(prev_st, inter_st); } //If the MinIK fails, then the steering is a failure, abort else { //Clear out whatever plan we had started making PRX_DEBUG_COLOR("IK failed in Steering", PRX_TEXT_BROWN); result_plan.clear(); return false; } } //Report our Success return true; }
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::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); }