Vector3f Arm::F(VectorXf theta) {
Vector3f rot1(theta(0), theta(1), theta(2));
Vector3f rot2(theta(3), theta(4), theta(5));
Vector3f rot3(theta(6), theta(7), theta(8));
Vector3f rot4(theta(9), theta(10), theta(11));
Matrix4f R1; R1 = rodrigues(rot1);
Matrix4f R2; R2 = rodrigues(rot2);
Matrix4f R3; R3 = rodrigues(rot3);
Matrix4f R4; R4 = rodrigues(rot4);
Matrix4f T1; T1 = list_joints[0]->transformation();
Matrix4f T2; T2 = list_joints[1]->transformation();
Matrix4f T3; T3 = list_joints[2]->transformation();
Matrix4f T4; T4 = list_joints[3]->transformation();
Vector4f identity(0.0, 0.0, 0.0, 1.0);
Vector4f result;
result = R1 * T1 * R2 * T2 * R3 * T3 * R4 * T4 * identity;
Vector3f ret(result(0), result(1), result(2));
// cout << "Theta:" << endl << theta;
// cout << "ret: " << endl << ret << endl;=
return ret;
}
bool SequencePlayer::setTargetPose(const char* gname, const double *xyz, const double *rpy, double tm, const char* frame_name)
{
if ( m_debugLevel > 0 ) {
std::cerr << __PRETTY_FUNCTION__ << std::endl;
}
Guard guard(m_mutex);
if (!setInitialState()) return false;
// setup
std::vector<int> indices;
hrp::dvector start_av, end_av;
std::vector<hrp::dvector> avs;
if (! m_seq->getJointGroup(gname, indices) ) {
std::cerr << "[setTargetPose] Could not find joint group " << gname << std::endl;
return false;
}
start_av.resize(indices.size());
end_av.resize(indices.size());
//std::cerr << std::endl;
if ( ! m_robot->joint(indices[0])->parent ) {
std::cerr << "[setTargetPose] " << m_robot->joint(indices[0])->name << " does not have parent" << std::endl;
return false;
}
string base_parent_name = m_robot->joint(indices[0])->parent->name;
string target_name = m_robot->joint(indices[indices.size()-1])->name;
// prepare joint path
hrp::JointPathExPtr manip = hrp::JointPathExPtr(new hrp::JointPathEx(m_robot, m_robot->link(base_parent_name), m_robot->link(target_name), dt, true, std::string(m_profile.instance_name)));
// calc fk
for (unsigned int i=0; i<m_robot->numJoints(); i++){
hrp::Link *j = m_robot->joint(i);
if (j) j->q = m_qRef.data.get_buffer()[i];
}
m_robot->calcForwardKinematics();
for ( unsigned int i = 0; i < manip->numJoints(); i++ ){
start_av[i] = manip->joint(i)->q;
}
// xyz and rpy are relateive to root link, where as pos and rotatoin of manip->calcInverseKinematics are relative to base link
// ik params
hrp::Vector3 start_p(m_robot->link(target_name)->p);
hrp::Matrix33 start_R(m_robot->link(target_name)->R);
hrp::Vector3 end_p(xyz[0], xyz[1], xyz[2]);
hrp::Matrix33 end_R = m_robot->link(target_name)->calcRfromAttitude(hrp::rotFromRpy(rpy[0], rpy[1], rpy[2]));
// change start and end must be relative to the frame_name
if ( (frame_name != NULL) && (! m_robot->link(frame_name) ) ) {
std::cerr << "[setTargetPose] Could not find frame_name " << frame_name << std::endl;
return false;
} else if ( frame_name != NULL ) {
hrp::Vector3 frame_p(m_robot->link(frame_name)->p);
hrp::Matrix33 frame_R(m_robot->link(frame_name)->attitude());
// fix start/end references from root to frame;
end_p = frame_R * end_p + frame_p;
end_R = frame_R * end_R;
}
manip->setMaxIKError(m_error_pos,m_error_rot);
manip->setMaxIKIteration(m_iteration);
std::cerr << "[setTargetPose] Solveing IK with frame" << frame_name << ", Error " << m_error_pos << m_error_rot << ", Iteration " << m_iteration << std::endl;
std::cerr << " Start " << start_p << start_R<< std::endl;
std::cerr << " End " << end_p << end_R<< std::endl;
// interpolate & calc ik
int len = max(((start_p - end_p).norm() / 0.02 ), // 2cm
((hrp::omegaFromRot(start_R.transpose() * end_R).norm()) / 0.025)); // 2 deg
len = max(len, 1);
std::vector<const double *> v_pos;
std::vector<double> v_tm;
v_pos.resize(len);
v_tm.resize(len);
// do loop
for (int i = 0; i < len; i++ ) {
double a = (1+i)/(double)len;
hrp::Vector3 p = (1-a)*start_p + a*end_p;
hrp::Vector3 omega = hrp::omegaFromRot(start_R.transpose() * end_R);
hrp::Matrix33 R = start_R * rodrigues(omega.isZero()?omega:omega.normalized(), a*omega.norm());
bool ret = manip->calcInverseKinematics2(p, R);
if ( m_debugLevel > 0 ) {
// for debug
std::cerr << "target pos/rot : " << i << "/" << a << " : "
<< p[0] << " " << p[1] << " " << p[2] << ","
<< omega[0] << " " << omega[1] << " " << omega[2] << std::endl;
}
if ( ! ret ) {
std::cerr << "[setTargetPose] IK failed" << std::endl;
return false;
}
v_pos[i] = (const double *)malloc(sizeof(double)*manip->numJoints());
for ( unsigned int j = 0; j < manip->numJoints(); j++ ){
((double *)v_pos[i])[j] = manip->joint(j)->q;
}
v_tm[i] = tm/len;
}
if ( m_debugLevel > 0 ) {
// for debug
for(int i = 0; i < len; i++ ) {
std::cerr << v_tm[i] << ":";
for(int j = 0; j < start_av.size(); j++ ) {
std::cerr << v_pos[i][j] << " ";
}
std::cerr << std::endl;
}
}
bool ret = m_seq->playPatternOfGroup(gname, v_pos, v_tm, m_qInit.data.get_buffer(), v_pos.size()>0?indices.size():0);
// clean up memory, need to improve
for (int i = 0; i < len; i++ ) {
free((double *)v_pos[i]);
}
return ret;
}
