Solver kernel#

The batched HJCD solver: coarse search + Levenberg–Marquardt refine, warp-per-candidate.

Defines

SYNC()#

Enums

Values:

enumerator N#

Functions

int grid_num_joints()#

void init_joint_limits_from_grid()#

template<typename T> void sample_joint_config(T *s_x, int local_problem, int global_problem)#

template<typename T> void perturb_joint_config(T *s_x, int global_problem, T sigma_frac = (T)0.05)#

template<typename T> void mat_to_quat(const T *C, T *q)#

template<typename T> void multiply_quat(const T *r, const T *s, T *t)#

template<typename T> void normalize_quat(T *quat)#

template<typename T> void quat_conj(const T *q, T *qc)#

template<typename T> void quat_mul(const T *a, const T *b, T *o)#

template<typename T> void quat_err_rotvec(const T *q_cur, const T *q_goal, T *w_err3)#

template<typename T> void normalize_vec3(T *vec)#

template<typename T> T compute_ori_err(const T *CjX, const T *q_goal)#

template<typename T> T compute_pos_err(const T *C, const T *target_pose)#

template<typename T> T compute_pos_err_at(const T *ee16, const T *target_pose)#

template<typename T> T compute_ori_err_at(const T *ee16, const T *q_goal)#

template<typename T> T solve_pos(const T *s_jointXforms, const T *pos, const T *target_pose_local, int joint, int k, int k_max, T delta_min = 0.35, T delta_max = 0.75)#

template<typename T> T solve_ori(const T *s_jointXforms, const T *q_t, int joint, int k, int k_max)#

void upper_index_to_rc(int idx, int DIM, int &r, int &c)#

template<typename T> T safe_normN(const T *v, int n)#

template<typename T> void recompute_cost_scaled(T *xcur, T *s_jointX, T *s_XmatsHom, const T *row_s, const T *tp, const T *q_goal, T &cost_sq, T &pos_err_m, T &ori_err_rad)#

template<typename T> bool try_dogleg_step(T *s_x, const T *x_old, T *s_jointX, T *s_XmatsHom, const T *row_s, const T *tp, const T *q_goal, const T R, const T *dq_gn, const T *gvec, const T *diagA, T &cost_sq, T &pos_err_m, T &ori_err_rad, T &lambda, const T lambda_min, const T lambda_max, const double2 *limits)#

template<typename T> bool try_coord_linesearch(T *s_x, const T *x_old, T *s_jointX, T *s_XmatsHom, const T *row_s, const T *tp, const T *q_goal, const T *gvec, const T R, const T pos_err_m_hint, T &cost_sq, T &pos_err_m, T &ori_err_rad, T &lambda, const T lambda_min, const T lambda_max, const double2 *limits)#

template<typename T, int DIM> inline void build_ne_and_solve_warp(const T *J, const T *r_scaled, T lambda, T *dq, T *diagA, T *gvec, T *A_sh, T *b_sh, int *s_fail_ptr)#

template<typename T> void solve_lm_batched(T *x, T *pose, const T *target_poses, T *pos_error, T *ori_error, const grid::robotModel<T> *d_robotModel, const T eps_pos, const T eps_ori, T lambda_init, const int k_max, const int B, int stop_on_first)#

template<typename T> void coarse_search(T *x, T *pose, const T *targetsB, T *pos_errors, T *ori_errors, const grid::robotModel<T> *d_robotModel, bool stop_on_first)#

template<typename T> void lm_tuner(T *x, T *pose, const T *targetsB, T *pos_errors, T *ori_errors, const grid::robotModel<T> *d_robotModel, T eps_pos_m, T eps_ori_rad, T lambda_init, int k_max, int B, int stop_on_first)#

template<typename T> void gather_rows_kernel(const T *xsrc, const int *idx, T *xdst, int rows)#

template<typename T> void forward_kinematics_kernel(const T *q, T *ee_pose7, T *all_link_T, const grid::robotModel<T> *RM, const int B)#

template<typename T> inline T radical_inverse(uint32_t n, int b)#

template<typename T> void sample_q_halton_kernel(T *d_q, int num_configs, uint64_t seed, int offset = 1, int leap = 1)#

template<typename T> T *sample_ik_config_halton(const grid::robotModel<T> *d_robotModel, int num_configs, uint64_t seed, int offset = 1, int leap = 1)#

template<typename T> std::vector<std::array<T, 7>> sample_random_target_poses(const grid::robotModel<T> *d_robotModel, int num_configs, uint64_t seed)#

template<typename T> void gather_rows_generic(const T *src, const int *idx, T *dst, int K, int C)#

template<typename T> void build_scores_kernel(const T *pos_err_mm, const T *ori_err_rad, T *scores, int B)#

template<typename T> void replicate_rows_kernel(const T *src, T *dst, int K, int C, int rep)#

template<typename T> void replicate_target7_kernel(const T *target7, T *out, int R)#

template<typename T> void perturb_rows_kernel(T *X, int R, T sigma_frac, uint64_t seed, int groupSize, bool skip_first_in_group)#

template<typename Dst, typename Src> void cast_array(const Src *in, Dst *out, size_t n)#

template<typename T, typename RT> Result<T> generate_ik_solutions(T *target_pose, const grid::robotModel<T> *d_robotModel, int b_size, int num_solutions, bool collision_free, const char *problems_json_text, const char *problem_set_name, int problem_idx, bool write_stats)#

template Result< double > generate_ik_solutions< double > (double *target_pose, const grid::robotModel< double > *d_robotModel, int b_size, int num_solutions, bool collision_free, const char *problems_json_text, const char *problem_set_name, int problem_idx, bool write_stats)

template Result< double > generate_ik_solutions< double, float > (double *target_pose, const grid::robotModel< double > *d_robotModel, int b_size, int num_solutions, bool collision_free, const char *problems_json_text, const char *problem_set_name, int problem_idx, bool write_stats)

template Result< float > generate_ik_solutions< float > (float *target_pose, const grid::robotModel< float > *d_robotModel, int b_size, int num_solutions, bool collision_free, const char *problems_json_text, const char *problem_set_name, int problem_idx, bool write_stats)

template std::vector< std::array< double, 7 > > sample_random_target_poses (const grid::robotModel< double > *d_robotModel, int num_configs, uint64_t seed)

template std::vector< std::array< float, 7 > > sample_random_target_poses (const grid::robotModel< float > *d_robotModel, int num_configs, uint64_t seed)

Variables

constexpr int FLANGE_IDX = N + 1#

constexpr int EE_IDX = N#

constexpr int NX = FLANGE_IDX + 1#

__constant__ double2 c_joint_limits [N]

__constant__ int c_halton_bases [32] ={2,3,5,7,11,13,17,19,23,29,31,37,41,43,47,53,59,61,67,71,73,79,83,89,97,101,103,107,109,113,127,131}

static constexpr int CC_TPB = 128#

const double ENV_COLLISION_COST_W = 1.5#

const double CC_HARD_PENALTY = 1e12#

const double ORI_TARGET_RAD = 1.1e-4#

const double ORI_OUTLIER_W = 7000.0#

const float CC_SPHERE_MARGIN_MM = 0.0f#

template<typename T> struct LMWarpScratch#

Public Members

T s_x[N]#

T x_old[N]#

T s_XmatsHom[grid::XHOM_T_COUNT]#

T s_jointX[NX * 16]#

T J[6 * N]#

T r_scaled[6]#

T row_s[6]#

T q_goal[4]#

T dq[N]#

T diagA[N]#

T gvec[N]#

T best_x_pos[N]#

T Ad_sh[N * N]#

T rhsd_sh[N]#

T row_norm2[6]#

T pos_err_m#

T ori_err_rad#

T cost_sq#

T prev_cost#

T best_pos_seen#

int s_break#

int stall#

int accepted#

int s_fail#

namespace grid#

Functions

template<typename T> T *init_joint_limits()#