int ChainIkSolverPos_NR_JL_Mimic::CartToJnt(const JntArray& q_init, const Frame& p_in, JntArray& q_out)
{
//Note that q_init and q_out will be of size chain.getNrOfJoints() - num_mimic_joints
qToqMimic(q_init,q_temp);
unsigned int i;
for(i=0;i<maxiter;++i)
{
fksolver.JntToCart(q_temp,f);
delta_twist = diff(f,p_in);
// if(Equal(delta_twist,Twist::Zero(),eps))
// break;
if(fabs(delta_twist(0)) < eps && fabs(delta_twist(1)) < eps && fabs(delta_twist(2)) < eps)
break;
ROS_DEBUG_STREAM("delta_twist");
for(std::size_t i=0; i < 6; ++i)
ROS_DEBUG("%d: %f",(int) i, delta_twist(i));
iksolver.CartToJnt(q_temp,delta_twist,delta_q);
Add(q_temp,delta_q,q_temp);
ROS_DEBUG_STREAM("delta_q");
for(std::size_t i=0; i < delta_q.rows(); ++i)
ROS_DEBUG("%d: %f",(int) i, delta_q(i));
for(std::size_t j=0; j<q_min_mimic.rows(); ++j)
{
if(mimic_joints[j].active)
if(q_temp(j) < q_min_mimic(j))
q_temp(j) = q_min_mimic(j);
}
for(std::size_t j=0; j<q_max_mimic.rows(); ++j)
{
if(mimic_joints[j].active)
if(q_temp(j) > q_max_mimic(j))
q_temp(j) = q_max_mimic(j);
}
//Make sure limits are applied on the mimic joints to
qMimicToq(q_temp,q_out);
qToqMimic(q_out,q_temp);
}
qMimicToq(q_temp, q_out);
ROS_DEBUG_STREAM("Full Solution:");
for(std::size_t i=0; i < q_temp.rows(); ++i)
ROS_DEBUG("%d: %f",(int) i,q_temp(i));
ROS_DEBUG_STREAM("Actual Solution:");
for(std::size_t i=0; i < q_out.rows(); ++i)
ROS_DEBUG("%d: %f",(int) i,q_out(i));
if(i!=maxiter)
return 0;
else
return -3;
}
void learning() {
std::vector<double> delta_p(k), delta_q(k);
for(auto & kv : usr_dct) {
p[kv.second] = random_double_list(k, 0.1);
usr_bias[kv.second] = 0.1 * random_double();
}
for(auto & kv : item_dct) {
q[kv.second] = random_double_list(k, 0.1);
item_bias[kv.second] = 0.1 * random_double();
}
std::cout << "init done" << std::endl;
auto learning_lambda = [&] (const std::string & uid,
const std::string & iid,
double rating) {
double e = rating - estimate(uid, iid);
// compute delta
for(int i = 0; i < k; ++i) {
delta_p[i] = alpha * (2 * e * q[iid][i] - beta * p[uid][i]);
delta_q[i] = alpha * (2 * e * p[uid][i] - beta * q[iid][i]);
}
// update with delta
for(int i = 0; i < k; ++i) {
p[uid][i] += delta_p[i];
q[iid][i] += delta_q[i];
}
// update bias
usr_bias[uid] += alpha * (2 * e - beta * usr_bias[uid]);
item_bias[iid] += alpha * (2 * e - beta * item_bias[iid]);
};
// learning
for(int rd = 0; rd < rounds; ++rd) {
std::cout << "rd:" << rd << " started" << std::endl;
rating_graph.traverse(learning_lambda);
} // end for
}