bool NAO_PLANNER_CONTROL::compute_motion_plan(rrt_planner_msgs::Compute_Motion_Plan::Request &req, rrt_planner_msgs::Compute_Motion_Plan::Response &resp)
{
std::cout <<"Compute Manipulation Plan requested"<< std::endl;
// //-----------Start Configuration (= current robot state in planning_scene)
// //Get the joint names of the planning group
// //const std::vector<std::string>& joint_names = p_s_->getRobotModel()->getJointModelGroup(PLANNING_GROUP)->getJointModelNames();
// //Get the current configuration (for the whole robot)
// robot_state::RobotState state = p_s_->getCurrentState();
// //Get Joint Names
// std::vector<std::string> joint_title;
// joint_title = state.getVariableNames();
// //Get Joint Values
// const double *joint_vals;
// joint_vals = state.getVariablePositions();
// std::map< std::string, double > joint_values;
// for (int i = 0; i < joint_title.size(); i++)
// {
// joint_values.insert ( std::pair<std::string,double>(joint_title[i],joint_vals[i]));
// }
// //state.getStateValues(joint_values);
// //Erase configuration elements not used for planning
// joint_values.erase("HeadPitch");
// joint_values.erase("HeadYaw");
// joint_values.erase("world_joint/theta");
// joint_values.erase("world_joint/x");
// joint_values.erase("world_joint/y");
// // //Plot remaining configuration
// // for(std::map<std::string,double>::const_iterator i = joint_values.begin(); i != joint_values.end(); ++i)
// // {
// // std::cout << i->first << ": " << i->second << std::endl;
// // }
// //Configuration for Planning (rearrange joint values in the order of the Kin. Model)
// std::vector<double> start_config(joint_names_.size());
// std::cout<<"Start Config."<<std::endl;
// for(unsigned int i = 0 ; i < joint_names_.size(); i++)
// {
// start_config[i] = joint_values[joint_names_[i]];
// std::cout << joint_names_[i] << ": " << joint_values[joint_names_[i]] << std::endl;
// }
//-----------Start Configuration (from request) -------------------------
std::vector<double> start_config(joint_names_.size());
std::cout<<"Start Config."<<std::endl;
for(unsigned int i = 0 ; i < joint_names_.size(); i++)
{
start_config[i] = req.wb_start_config[i];
std::cout << joint_names_[i] << ": " << start_config[i] << std::endl;
}
//Set trajectory mode (false will prepare a trajectory for simulation, true will prepare a trajectory for execution)
planner_->setTrajectoryMode(req.execute_trajectory);
//-----------Goal Configuration (generated by SCG)
std::vector<double> desired_right_hand_pose(6);
//Set the desired position of the right hand w.r.t the right foot
desired_right_hand_pose[0] = req.right_hand_position.x; // X [m]
desired_right_hand_pose[1] = req.right_hand_position.y; // Y [m]
desired_right_hand_pose[2] = req.right_hand_position.z; // Z [m]
//Set the desired direction of the z-axis of the right hand w.r.t the right foot frame
desired_right_hand_pose[3] = req.right_hand_direction.x; // X component of hand z-axis
desired_right_hand_pose[4] = req.right_hand_direction.y; // Y component of hand z-axis
desired_right_hand_pose[5] = req.right_hand_direction.z; // Z component of hand z-axis
//Sample a goal configuration
scg_->setDesiredRightArmPose(desired_right_hand_pose);
int num_goal_configs_found = 0;
float time_for_first_config = 0.0;
num_goal_configs_found = scg_->sample_stable_configs(MAX_SAMPLES,MAX_IK_ITERATIONS,time_for_first_config, true, SSC_DS_GOAL_CONFIG_FIRST);
//Time required to find first config
resp.time_goal_config = time_for_first_config;
if (num_goal_configs_found > 0)
{
//Get the best goal config from the file
std::vector<double> goal_config(joint_names_.size());
scg_->loadGoalConfig(goal_config, SSC_DS_GOAL_CONFIG_FIRST);
//Assign best goal config to service response
resp.wb_goal_config = goal_config;
//-----------Planner
//Set joint weights
std::vector<double> joint_weights(joint_names_.size());
//Set joint weights
double r_arm_weight = R_ARM_WEIGHT_APPROACH;
double l_arm_weight = L_ARM_WEIGHT_APPROACH;
double leg_weight = LEGS_WEIGHT_APPROACH;
joint_weights[0] = leg_weight; // RAnkleRoll
joint_weights[1] = leg_weight; // RAnklePitch
joint_weights[2] = leg_weight; // RKneePitch
joint_weights[3] = leg_weight; // RHipPitch
joint_weights[4] = leg_weight; // RHipRoll
joint_weights[5] = l_arm_weight; // LShoulderPitch
joint_weights[6] = l_arm_weight; // LShoulderRoll
joint_weights[7] = l_arm_weight; // LElbowYaw
joint_weights[8] = l_arm_weight; // LElbowRoll
joint_weights[9] = l_arm_weight; // LWristYaw
joint_weights[10] = r_arm_weight; // RShoulderPitch
joint_weights[11] = r_arm_weight; // RShoulderRoll
joint_weights[12] = r_arm_weight; // RElbowYaw
joint_weights[13] = r_arm_weight; // RElbowRoll
joint_weights[14] = r_arm_weight; // RWristYaw
joint_weights[15] = 0.0; // LHipYawPitch
joint_weights[16] = leg_weight; // LHipRoll
joint_weights[17] = leg_weight; // LHipPitch
joint_weights[18] = leg_weight; // LKneePitch
joint_weights[19] = leg_weight; // LAnklePitch
joint_weights[20] = leg_weight; // LAnkleRoll
planner_->setJointWeights(joint_weights);
std::cout<<"Right before setStartGoalConfigs in nao_planner_control"<<std::endl;
if (planner_->setStartGoalConfigs(start_config,goal_config))
{
//Solve the query
int num_nodes = 0;
float time_elapsed = 0.0;
resp.motion_plan = planner_->solveQuery(MAX_EXPAND_ITERATIONS, ADVANCE_STEPS,SOLUTION_FILE_PATH_FIRST,num_nodes,time_elapsed);
resp.num_nodes_generated = num_nodes;
resp.search_time = time_elapsed;
}
if(resp.motion_plan.trajectory.size()>0 && resp.motion_plan.trajectory[0].joint_trajectory.points.size() > 0)
resp.success = true;
else
resp.success = false;
}
else
{
resp.success = false;
}
return true;
}
bool NAO_PLANNER_CONTROL::compute_circular_manipulation_plan(rrt_planner_msgs::Compute_Circular_Manipulation_Plan::Request &req, rrt_planner_msgs::Compute_Circular_Manipulation_Plan::Response &resp)
{
std::cout <<"Circular Manipulation Plan requested"<< std::endl;
// //-----------Start Configuration (= current robot state in planning_scene)
// //Get the joint names of the planning group
// //const std::vector<std::string>& joint_names = p_s_->getRobotModel()->getJointModelGroup(PLANNING_GROUP)->getJointModelNames();
// //Get the current configuration (for the whole robot)
// //planning_scene_monitor::LockedPlanningSceneRO p_s(psm_);
// robot_state::RobotState state = p_s_->getCurrentState();
// //Get Joint Names
// std::vector<std::string> joint_title;
// joint_title = state.getVariableNames();
// //Get Joint Values
// const double *joint_vals;
// joint_vals = state.getVariablePositions();
// std::map< std::string, double > joint_values;
// for (int i = 0; i < joint_title.size(); i++)
// {
// joint_values.insert ( std::pair<std::string,double>(joint_title[i],joint_vals[i]));
// }
// //std::map< std::string, double > joint_values;
// //state.getStateValues(joint_values);
// //Erase configuration elements not used for planning
// joint_values.erase("HeadPitch");
// joint_values.erase("HeadYaw");
// joint_values.erase("world_joint/theta");
// joint_values.erase("world_joint/x");
// joint_values.erase("world_joint/y");
// //Plot remaining configuration
// // for(std::map<std::string,double>::const_iterator i = joint_values.begin(); i != joint_values.end(); ++i)
// // {
// // std::cout << i->first << ": " << i->second << std::endl;
// // }
// //Configuration for Planning (rearrange joint values in the order of the Kin. Model)
// std::vector<double> start_config(joint_names_.size());
// for(unsigned int i = 0 ; i < joint_names_.size(); i++)
// {
// start_config[i] = joint_values[joint_names_[i]];
// //std::cout << joint_names[i] << ": " << joint_values[joint_names_[i]] << std::endl;
// }
//-----------Start Configuration (from request) -------------------------
std::vector<double> start_config(joint_names_.size());
std::cout<<"Start Config."<<std::endl;
for(unsigned int i = 0 ; i < joint_names_.size(); i++)
{
start_config[i] = req.wb_start_config[i];
std::cout << joint_names_[i] << ": " << start_config[i] << std::endl;
}
//-----------Init Planning variables
//Weights for limb joints
double r_arm_weight = 0.0;
double l_arm_weight = 0.0;
double leg_weight = 0.0;
//Weight array
std::vector<double> joint_weights(joint_names_.size());
//Array storing the goal config for the approach motion plan
std::vector<double> goal_config_approach(joint_names_.size());
//Array storing the goal config for the manipulate motion plan
std::vector<double> goal_config_manipulate(joint_names_.size());
//Set trajectory mode (false will prepare a trajectory for simulation, true will prepare a trajectory for execution)
planner_->setTrajectoryMode(req.execute_trajectory);
//------------------ Motion Planning Approach
//Goal Configuration for approach (generated by SCG)
std::vector<double> desired_right_hand_pose(6);
//Set the desired position of the right hand w.r.t the right foot
desired_right_hand_pose[0] = req.right_hand_position.x; // X [m]
desired_right_hand_pose[1] = req.right_hand_position.y; // Y [m]
desired_right_hand_pose[2] = req.right_hand_position.z; // Z [m]
//Set the desired direction of the z-axis of the right hand w.r.t the right foot frame
desired_right_hand_pose[3] = req.right_hand_direction.x; // X component of hand z-axis
desired_right_hand_pose[4] = req.right_hand_direction.y; // Y component of hand z-axis
desired_right_hand_pose[5] = req.right_hand_direction.z; // Z component of hand z-axis
//Sample a Goal Configuration
scg_->setDesiredRightArmPose(desired_right_hand_pose);
int num_goal_configs_found = 0;
float time_for_first_config = 0.0;
num_goal_configs_found = scg_->sample_stable_configs(MAX_SAMPLES,MAX_IK_ITERATIONS,time_for_first_config, true, SSC_DS_GOAL_CONFIG_FIRST);
//Time required to find first config
resp.time_goal_config_approach = time_for_first_config;
//Proceed only when a goal config has been found
if (num_goal_configs_found > 0)
{
//Get the best goal config from the file
scg_->loadGoalConfig(goal_config_approach, SSC_DS_GOAL_CONFIG_FIRST);
//Assign best goal config to service response
resp.wb_goal_config_approach = goal_config_approach;
//Set joint weights
r_arm_weight = R_ARM_WEIGHT_APPROACH;
l_arm_weight = L_ARM_WEIGHT_APPROACH;
leg_weight = LEGS_WEIGHT_APPROACH;
joint_weights[0] = leg_weight; // RAnkleRoll
joint_weights[1] = leg_weight; // RAnklePitch
joint_weights[2] = leg_weight; // RKneePitch
joint_weights[3] = leg_weight; // RHipPitch
joint_weights[4] = leg_weight; // RHipRoll
joint_weights[5] = l_arm_weight; // LShoulderPitch
joint_weights[6] = l_arm_weight; // LShoulderRoll
joint_weights[7] = l_arm_weight; // LElbowYaw
joint_weights[8] = l_arm_weight; // LElbowRoll
joint_weights[9] = l_arm_weight; // LWristYaw
joint_weights[10] = r_arm_weight; // RShoulderPitch
joint_weights[11] = r_arm_weight; // RShoulderRoll
joint_weights[12] = r_arm_weight; // RElbowYaw
joint_weights[13] = r_arm_weight; // RElbowRoll
joint_weights[14] = r_arm_weight; // RWristYaw
joint_weights[15] = 0.0; // LHipYawPitch
joint_weights[16] = leg_weight; // LHipRoll
joint_weights[17] = leg_weight; // LHipPitch
joint_weights[18] = leg_weight; // LKneePitch
joint_weights[19] = leg_weight; // LAnklePitch
joint_weights[20] = leg_weight; // LAnkleRoll
planner_->setJointWeights(joint_weights);
if (planner_->setStartGoalConfigs(start_config,goal_config_approach))
{
//Solve the query
int num_nodes = 0;
float time_elapsed = 0.0;
resp.motion_plan_approach = planner_->solveQuery(MAX_EXPAND_ITERATIONS, ADVANCE_STEPS,SOLUTION_FILE_PATH_FIRST, num_nodes,time_elapsed);
resp.num_nodes_generated_approach = num_nodes;
resp.search_time_approach = time_elapsed;
}
if(resp.motion_plan_approach.trajectory.size() > 0 && resp.motion_plan_approach.trajectory[0].joint_trajectory.points.size() > 0)
resp.success_approach = true;
else
resp.success_approach = false;
}
else
{
resp.success_approach = false;
}
//------------------ Motion Planning Manipulation
if (resp.success_approach == true)
{
//Goal Configuration for manipulation (generated by SCG)
//From handle position to point on the door
double x_door = desired_right_hand_pose[0] + req.clearance_handle * cos(req.relative_angle*(M_PI/180));
double y_door = desired_right_hand_pose[1] - req.clearance_handle * sin(req.relative_angle*(M_PI/180));
double z_door = desired_right_hand_pose[2];
//From point on the door to hinge position
double x_hinge = x_door - req.rotation_radius * sin(req.relative_angle*(M_PI/180));
double y_hinge = y_door - req.rotation_radius * cos(req.relative_angle*(M_PI/180));
double z_hinge = z_door;
//Goal position for point on the door
double tmp_angle = (req.rotation_angle-req.relative_angle) *(M_PI/180);
x_door = x_hinge - req.rotation_radius * sin(tmp_angle); // X [m];
y_door = y_hinge + req.rotation_radius * cos(tmp_angle); // Y [m];
z_door = z_hinge; // Z [m];
//Goal position for door handle
tmp_angle = 1.5708 - tmp_angle; // 90 degree - tmp_angle
std::vector<double> desired_right_hand_pose_manipulate(6);
desired_right_hand_pose_manipulate[0] = x_door - req.clearance_handle * sin(tmp_angle);
desired_right_hand_pose_manipulate[1] = y_door - req.clearance_handle * cos(tmp_angle);
desired_right_hand_pose_manipulate[2] = z_door;
//Set the desired direction of the z-axis of the right hand w.r.t the right foot frame
desired_right_hand_pose_manipulate[3] = req.right_hand_direction.x; // X component of hand z-axis
desired_right_hand_pose_manipulate[4] = req.right_hand_direction.y; // Y component of hand z-axis
desired_right_hand_pose_manipulate[5] = req.right_hand_direction.z; // Z component of hand z-axis
//Sample a Goal Configuration
scg_->setDesiredRightArmPose(desired_right_hand_pose_manipulate);
time_for_first_config = 0.0;
num_goal_configs_found = scg_->sample_stable_configs(MAX_SAMPLES,MAX_IK_ITERATIONS,time_for_first_config, true, SSC_DS_GOAL_CONFIG_SECOND);
//Time required to find first config
resp.time_goal_config_manipulate = time_for_first_config;
if (num_goal_configs_found > 0)
{
//Get the best goal config from the file
scg_->loadGoalConfig(goal_config_manipulate, SSC_DS_GOAL_CONFIG_SECOND);
//Assign best goal config to service response
resp.wb_goal_config_manipulate = goal_config_manipulate;
//Parameters for the door (required)
std::vector<double> object_parameter(4);
object_parameter[0] = req.rotation_radius; //door width
object_parameter[1] = req.rotation_angle; //door opening angle
object_parameter[2] = req.relative_angle; //relative angle of door w.r.t right foot frame
object_parameter[3] = req.clearance_handle; // distance between door and handle
//Activate manipulation constraint
planner_->activate_door_constraint(desired_right_hand_pose,desired_right_hand_pose_manipulate,20,object_parameter);
//Set joint weights
r_arm_weight = R_ARM_WEIGHT_MANIPULATE;
l_arm_weight = L_ARM_WEIGHT_MANIPULATE;
leg_weight = LEGS_WEIGHT_MANIPULATE;
joint_weights[0] = leg_weight; // RAnkleRoll
joint_weights[1] = leg_weight; // RAnklePitch
joint_weights[2] = leg_weight; // RKneePitch
joint_weights[3] = leg_weight; // RHipPitch
joint_weights[4] = leg_weight; // RHipRoll
joint_weights[5] = l_arm_weight; // LShoulderPitch
joint_weights[6] = l_arm_weight; // LShoulderRoll
joint_weights[7] = l_arm_weight; // LElbowYaw
joint_weights[8] = l_arm_weight; // LElbowRoll
joint_weights[9] = l_arm_weight; // LWristYaw
joint_weights[10] = r_arm_weight; // RShoulderPitch
joint_weights[11] = r_arm_weight; // RShoulderRoll
joint_weights[12] = r_arm_weight; // RElbowYaw
joint_weights[13] = r_arm_weight; // RElbowRoll
joint_weights[14] = r_arm_weight; // RWristYaw
joint_weights[15] = 0.0; // LHipYawPitch
joint_weights[16] = leg_weight; // LHipRoll
joint_weights[17] = leg_weight; // LHipPitch
joint_weights[18] = leg_weight; // LKneePitch
joint_weights[19] = leg_weight; // LAnklePitch
joint_weights[20] = leg_weight; // LAnkleRoll
planner_->setJointWeights(joint_weights);
if (planner_->setStartGoalConfigs(goal_config_approach,goal_config_manipulate))
{
//Solve the query
int num_nodes = 0;
float time_elapsed = 0.0;
resp.motion_plan_manipulation = planner_->solveQuery(MAX_EXPAND_ITERATIONS, ADVANCE_STEPS,SOLUTION_FILE_PATH_SECOND, num_nodes,time_elapsed);
resp.num_nodes_generated_manipulate = num_nodes;
resp.search_time_manipulate = time_elapsed;
}
if(resp.motion_plan_manipulation.trajectory.size() > 0 && resp.motion_plan_manipulation.trajectory[0].joint_trajectory.points.size() > 0)
{
resp.success_manipulation = true;
}
else
resp.success_manipulation = false;
} //END if num_goal_configs_found > 0
else
{
resp.success_manipulation = false;
}
}//END if resp.success == true
return true;
}
double IK::solve_ik()
{
int i, j;
double current_max_condnum = -1.0;
copy_jacobian();
if(n_all_const > 0)
{
////
// check rank when HIGH_IF_POSSIBLE constraints have high priority
int n_high_const;
fMat Jhigh;
fMat wJhigh;
fVec fb_high;
fVec weight_high;
int* is_high_const = 0;
double cond_number = 1.0;
// cerr << "---" << endl;
// cerr << n_const[HIGH_PRIORITY] << " " << n_const[HIGH_IF_POSSIBLE] << " " << n_const[LOW_PRIORITY] << endl;
if(n_const[HIGH_IF_POSSIBLE] > 0)
{
is_high_const = new int [n_const[HIGH_IF_POSSIBLE]];
// initialize
for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
is_high_const[i] = true;
// search
int search_phase = 0;
while(1)
{
n_high_const = n_const[HIGH_PRIORITY];
for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
{
if(is_high_const[i]) n_high_const++;
}
Jhigh.resize(n_high_const, n_dof);
wJhigh.resize(n_high_const, n_dof);
fb_high.resize(n_high_const);
weight_high.resize(n_high_const);
if(n_const[HIGH_PRIORITY] > 0)
{
// set fb and J of higher priority pins
for(i=0; i<n_const[HIGH_PRIORITY]; i++)
{
fb_high(i) = fb[HIGH_PRIORITY](i);
weight_high(i) = weight[HIGH_PRIORITY](i);
for(j=0; j<n_dof; j++)
{
Jhigh(i, j) = J[HIGH_PRIORITY](i, j);
wJhigh(i, j) = Jhigh(i, j) * weight_high(i) / joint_weights(j);
}
}
}
int count = 0;
// set fb and J of medium priority pins
for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
{
if(is_high_const[i])
{
fb_high(n_const[HIGH_PRIORITY]+count) = fb[HIGH_IF_POSSIBLE](i);
weight_high(n_const[HIGH_PRIORITY]+count) = weight[HIGH_IF_POSSIBLE](i);
for(j=0; j<n_dof; j++)
{
Jhigh(n_const[HIGH_PRIORITY]+count, j) = J[HIGH_IF_POSSIBLE](i, j);
wJhigh(n_const[HIGH_PRIORITY]+count, j) = J[HIGH_IF_POSSIBLE](i, j) * weight[HIGH_IF_POSSIBLE](i) / joint_weights(j);
}
count++;
}
}
// singular value decomposition
fMat U(n_high_const, n_high_const), VT(n_dof, n_dof);
fVec s;
int s_size;
if(n_high_const < n_dof) s_size = n_high_const;
else s_size = n_dof;
s.resize(s_size);
wJhigh.svd(U, s, VT);
double condnum_limit = max_condnum * 100.0;
if(s(s_size-1) > s(0)/(max_condnum*condnum_limit))
cond_number = s(0) / s(s_size-1);
else
cond_number = condnum_limit;
if(current_max_condnum < 0.0 || cond_number > current_max_condnum)
{
current_max_condnum = cond_number;
}
if(n_high_const <= n_const[HIGH_PRIORITY]) break;
// remove some constraints
if(cond_number > max_condnum)
{
int reduced = false;
for(i=n_constraints-1; i>=0; i--)
{
if(constraints[i]->enabled &&
constraints[i]->priority == HIGH_IF_POSSIBLE &&
constraints[i]->i_const >= 0 &&
constraints[i]->GetType() == HANDLE_CONSTRAINT &&
is_high_const[constraints[i]->i_const])
{
IKHandle* h = (IKHandle*)constraints[i];
if(search_phase ||
(!search_phase && h->joint->DescendantDOF() > 0))
{
for(j=0; j<constraints[i]->n_const; j++)
{
is_high_const[constraints[i]->i_const + j] = false;
}
constraints[i]->is_dropped = true;
// cerr << "r" << flush;
reduced = true;
break;
}
}
}
if(!reduced) search_phase++;
}
else break;
}
}
else
{
n_high_const = n_const[HIGH_PRIORITY];
Jhigh.resize(n_high_const, n_dof);
wJhigh.resize(n_high_const, n_dof);
fb_high.resize(n_high_const);
weight_high.resize(n_high_const);
if(n_high_const > 0)
{
Jhigh.set(J[HIGH_PRIORITY]);
fb_high.set(fb[HIGH_PRIORITY]);
weight_high.set(weight[HIGH_PRIORITY]);
}
}
#if 0
////
// adjust feedback according to the condition number
if(current_max_condnum > max_condnum)
fb_high.zero();
else
{
double k = (current_max_condnum-max_condnum)/(1.0-max_condnum);
fb_high *= k;
cerr << current_max_condnum << ", " << k << endl;
}
////
////
if(current_max_condnum < 0.0) current_max_condnum = 1.0;
#endif
int n_low_const = n_all_const - n_high_const;
int low_first = 0, count = 0;
fMat Jlow(n_low_const, n_dof);
fVec fb_low(n_low_const);
fVec weight_low(n_low_const);
for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
{
if(!is_high_const[i])
{
fb_low(count) = fb[HIGH_IF_POSSIBLE](i);
weight_low(count) = weight[HIGH_IF_POSSIBLE](i);
for(j=0; j<n_dof; j++)
{
Jlow(count, j) = J[HIGH_IF_POSSIBLE](i, j);
}
count++;
}
}
low_first = count;
double* p = fb_low.data() + low_first;
double* q = fb[LOW_PRIORITY].data();
double* r = weight_low.data() + low_first;
double* s = weight[LOW_PRIORITY].data();
for(i=0; i<n_const[LOW_PRIORITY]; p++, q++, r++, s++, i++)
{
// fb_low(low_first+i) = fb[LOW_PRIORITY](i);
*p = *q;
*r = *s;
double* a = Jlow.data() + low_first + i;
int a_row = Jlow.row();
double* b = J[LOW_PRIORITY].data() + i;
int b_row = J[LOW_PRIORITY].row();
for(j=0; j<n_dof; a+=a_row, b+=b_row, j++)
{
// Jlow(low_first+i, j) = J[LOW_PRIORITY](i, j);
*a = *b;
}
}
if(is_high_const) delete[] is_high_const;
fVec jvel(n_dof); // ³û¼â1¡¦x
fVec jvel0(n_dof); // ¹ó/¡¦¾å
fVec fb_low_0(n_low_const), dfb(n_low_const), y(n_dof);
fMat Jinv(n_dof, n_high_const), W(n_dof, n_dof), JW(n_low_const, n_dof);
fVec w_error(n_low_const), w_norm(n_dof);
// weighted
double damping = 0.1;
// w_error = 1.0;
w_error.set(weight_low);
w_norm = 1.0;
// w_norm.set(joint_weights);
// cerr << "joint_weights = " << joint_weights << endl;
// cerr << "weight_high = " << weight_high << endl;
// cerr << "weight_low = " << weight_low << endl;
#ifdef MEASURE_TIME
clock_t t1 = clock();
#endif
// ¹ó/¡¦¾å¡¦€ÅæéòÉçïàïêvZ
if(n_high_const > 0)
{
for(i=0; i<n_dof; i++)
{
int a_row = wJhigh.row();
double* a = wJhigh.data() + a_row*i;
int b_row = Jhigh.row();
double* b = Jhigh.data() + b_row*i;
double* c = joint_weights.data() + i;
double* d = weight_high.data();
for(j=0; j<n_high_const; a++, b++, d++, j++)
{
// wJhigh(j, i) = Jhigh(j, i) / joint_weights(i);
*a = *b * *d / *c;
}
}
fVec w_fb_high(fb_high);
for(i=0; i<n_high_const; i++)
w_fb_high(i) *= weight_high(i);
Jinv.p_inv(wJhigh);
jvel0.mul(Jinv, w_fb_high);
W.mul(Jinv, wJhigh);
for(i=0; i<n_dof; i++)
{
double* w = W.data() + i;
double a = joint_weights(i);
for(j=0; j<n_dof; w+=n_dof, j++)
{
if(i==j) *w -= 1.0;
*w /= -a;
}
jvel0(i) /= a;
}
JW.mul(Jlow, W);
}
else
{
jvel0.zero();
JW.set(Jlow);
}
#ifdef MEASURE_TIME
clock_t t2 = clock();
high_constraint_time += (double)(t2-t1)/(double)CLOCKS_PER_SEC;
#endif
// Ǥ¡¦#x¥¯¥È¥ë¹à¤ê³×Z
if(n_low_const > 0)
{
fb_low_0.mul(Jlow, jvel0);
dfb.sub(fb_low, fb_low_0);
y.lineq_sr(JW, w_error, w_norm, damping, dfb);
if(n_high_const > 0) jvel.mul(W, y);
else jvel.set(y);
// fVec error(n_low_const);
// error = dfb-Jlow*jvel;
// cerr << dfb*dfb << "->" << error*error << endl;
}
else
{
jvel.zero();
}
// ³û¼â1¡¦x
jvel += jvel0;
#ifdef MEASURE_TIME
clock_t t3 = clock();
low_constraint_time += (double)(t3-t2)/(double)CLOCKS_PER_SEC;
#endif
SetJointVel(jvel);
// cerr << fb_high - Jhigh * jvel << endl;
}
if(current_max_condnum < 0.0) current_max_condnum = 1.0;
return current_max_condnum;
}
