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; }