//Recursively skip a p. Return a pointer right after it, or a pointer on the
//start of the p to skip if the 0 terminating char is encountered before the
//end of the p.
static u8 *end_p(u8 *p)
{
u8 *pc=p;
loop
if(*++pc=='\0') return p;//this is an invalid p
else if(*pc=='['){//handle brackets special
//Skip the not sign. We have to recognize it because of a possibly
//following ']'.
if(*++pc=='!'||(*pc=='^')) ++pc;
if(*pc==']') ++pc;//a leading ']' is recognized as such
///skip over all chars of the list
while(1){
if(*pc==']') break;
if(*pc++=='\0') return p;//this is no valid p
}
}else if((*pc=='?'||*pc=='*'||*pc=='+'||*pc=='@'||*pc=='!')&&pc[1]=='(')
pc=end_p(pc+1);
else if(*pc==')') break;
return pc+1;
}
static u16 star_handler(u8 *c,u8 **pc,u8 **sc,u8 *str_end,u8 flgs,
struct match_params *end_star)
{
if(flgs&EXTMATCH&&**pc=='('){
s8 r=extended_match_try(*c,*pc,*sc,str_end,flgs);
if(r!=NO_VALID_P) return RETURN_R|R_SET(r);
}else if(end_star){/*this star will exit the current star context matching*/
end_star->p=*pc-1;
end_star->str=*sc;
return RETURN_R|R_SET(MATCH);
}
loop{
*c=*(*pc)++;
/*do "absorb" in the current * the following sequence of ?,*,?()
and *()*/
if(*c!='?'&&*c!='*') break;
/*?() and *() are "zero or one|more" occurences of matched patterns,
then absorbed them in the **/
if(**pc=='('&&(flgs&EXTMATCH)){
u8 *endp=end_p(*pc);
if(endp!=*pc){/*This is a p. Skip over it*/
*pc=endp;
continue;
}
}
if(*c=='?'){/*match one any char from the string*/
/*a ? needs to match one char*/
if(*sc==str_end)/*there isn't another char; no match*/
return RETURN_R|R_SET(NOMATCH);
else
/*one char of the str is consumed in matching this ? wildcard, so
*??? won't match if there are less than three chars*/
++(*sc);
}
}
/*the wildcard(s) is/are the last element of the p flag is set*/
if(*c=='\0') return RETURN_R|R_SET(MATCH);
struct match_params local_end_star;
local_end_star.p=0;
if(*c=='['||(flgs&EXTMATCH&&(*c=='@'||*c=='+'||*c=='!')&&**pc=='(')){
/*the * is followed by a bracket or an "not absorbed" ep*/
(*pc)--;
/*See explanation of star context matching right below. Instead of a char
like below, it's an ep.*/
loop{
if(*sc==str_end) break;
s8 r=match(*pc,*sc,str_end,flgs,&local_end_star);
if(r==MATCH){
if(local_end_star.p==0) return RETURN_R|R_SET(MATCH);
break;
}
++(*sc);
}
}else{/*the * is followed by a "normal" char*/
static u16 star_handler(u8 *c,u8 **pc,u8 **sc,void *str_end,u8 flgs,
struct match_params *end_star)
{
if(flgs&EXTMATCH&&**pc=='('){
s8 r=extended_match_try(*c,*pc,*sc,str_end,flgs);
if(r!=NO_VALID_P) return RETURN_R|R_SET(r);
}else if(end_star){//this star will exit the current star context matching
end_star->p=*pc-1;
end_star->str=*sc;
return RETURN_R|R_SET(MATCH);
}
while(1){
*c=*(*pc)++;
//do "absorb" in the current * the following sequence of ?,*,?()
//and *()
if(*c!='?'&&*c!='*') break;
//?() and *() are "zero or one|more" occurences of matched patterns,
//then absorbed them in the *
if(**pc=='('&&(flgs&EXTMATCH)){
u8 *endp=end_p(*pc);
if(endp!=*pc){//This is a p. Skip over it.
*pc=endp;
continue;
}
}
if(*c=='?'){//match one any char from the string
//a ? needs to match one char
if(*sc==str_end)//there isn't another char; no match
return RETURN_R|R_SET(NOMATCH);
else
//one char of the str is consumed in matching this ? wildcard, so
//*??? won't match if there are less than three chars
++(*sc);
}
}
//the wildcard(s) is/are the last element of the p flag is set
if(*c=='\0') return RETURN_R|R_SET(MATCH);
struct match_params local_end_star;
local_end_star.p=0;
if(*c=='['||(flgs&EXTMATCH&&(*c=='@'||*c=='+'||*c=='!')&&**pc=='(')){
//the * is followed by a bracket or an "not absorbed" ep
(*pc)--;
//See explanation of star context matching right below. Instead of a char
//like below, it's an ep.
while(1){
if(*sc==str_end) break;
s8 r=match(*pc,*sc,str_end,flgs,&local_end_star);
if(r==MATCH){
if(local_end_star.p==0) return RETURN_R|R_SET(MATCH);
break;
}
++(*sc);
}
}else{//the * is followed by a "normal" char
if(*c=='\\') *c=**pc;
*c=fold(*c,flgs);
(*pc)--;
//scan the str till we find a char which is the same than the first
//p char right after the *, then start to match in star context, namely
//till we reach the end, or we find another * will will end the matching
//in that context.
while(1){
if(*sc==str_end) break;
if(fold(**sc,flgs)==*c){
s8 r=match(*pc,*sc,str_end,flgs,&local_end_star);
if(r==MATCH){
if(local_end_star.p==0) return RETURN_R|R_SET(MATCH);
break;
}
}
++(*sc);
}
if(local_end_star.p!=0){//we are finish to match the str in star context
*pc=local_end_star.p;
*sc=local_end_star.str;
return NEXT_P_CHAR;
}
}
//if we come here no match is possible with the wildcard
return RETURN_R|R_SET(NOMATCH);
}
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;
}