Robots/YouBot/YouBotKinematics.cpp
// Inverse given a transform
// Default uses the safe yaw
Inverse given a transform
Default uses the safe yaw
Inverse given a transform Default uses the safe yaw
std::vector<double> YouBot_kinematics_inverse_arm(Transform tr, std::vector<double>& q, char& is_reach_back, bool use_safe_yaw=true) {
  double dx, dy, dz, base_yaw, pseudo_yaw, pitch, hand_yaw, tmp1, xy_coord;
	char unique_pitch, yaw_issue;
	tmp1 = tr( 2, 2 );
	unique_pitch = tmp1 > .999 ? 1 : (tmp1 < -.999 ? -1 : 0);
	if( unique_pitch!=0 ){
		pitch = unique_pitch > 0 ? 0 : PI;
		pseudo_yaw = 0;
		hand_yaw = asin( tr( 1, 0 ) );
#ifdef DEBUG
		printf("\nUndefined pitch %d\n",unique_pitch);
#endif
	} else {
		pitch = acos( tmp1 );
		pseudo_yaw = atan2( tr(1,2),tr(0,2) );
		hand_yaw = atan2( tr(2,1),-tr(2,0) );
#ifdef DEBUG
		printf("\nWell defined pitch\n");
#endif
	}
  dx = tr(0,3);
  dy = tr(1,3);
  dz = tr(2,3);
  xy_coord = sqrt(dx*dx + dy*dy);
	if( xy_coord<1e-9 ) {
		base_yaw = q[0];
	} else {
		base_yaw = atan2(dy,dx);
		if( (base_yaw-q[0])>=PI_ALMOST ){
#ifdef DEBUG
			printf("\tReach back +\n");
#endif
			base_yaw -= PI;
			xy_coord *= -1;
			is_reach_back = 1;
		} else if( (q[0]-base_yaw)>=PI_ALMOST ){
#ifdef DEBUG
			printf("\tReach back -\n");
#endif
			base_yaw += PI;
			xy_coord *= -1;
			is_reach_back = -1;
		} else
			is_reach_back = 0;
	} 
#ifdef DEBUG
	printf("\tbase_yaw: %lf\n",base_yaw);
	printf("\tq[0]: %lf\n",q[0]);
	printf("\treach_back: %d\n",is_reach_back);
	printf("\tpitch: %lf\n",pitch);
	printf("\tp_yaw: %lf\n",pseudo_yaw);
	printf("\th_yaw: %lf\n",hand_yaw);
#endif
	yaw_issue = 0;
	if( (base_yaw-pseudo_yaw)>=PI_HALF ){
#ifdef DEBUG
		printf("\tYaw issue +\n");
#endif
		pseudo_yaw += PI;
		hand_yaw -= PI;
		pitch *= -1;
		yaw_issue = 1;
	} else if( (pseudo_yaw-base_yaw)>=PI_HALF ){
#ifdef DEBUG
		printf("\tYaw issue -\n");
#endif
		pseudo_yaw -= PI;
		hand_yaw -= PI;
		pitch *= -1;
		yaw_issue = -1;
	}
#ifdef DEBUG
	printf("\tpitch: %lf\n",pitch);
	printf("\tbase_yaw: %lf\n",base_yaw);
	printf("\tpseudo_yaw: %lf\n",pseudo_yaw);
	printf("\thand_yaw: %lf\n",hand_yaw);
#endif
	if( pitch<0 && is_reach_back==0 ){
		pitch += PI_DOUBLE;
	}
	if(hand_yaw>PI)
		hand_yaw -= PI_DOUBLE;
	else if(hand_yaw<-PI)
		hand_yaw += PI_DOUBLE;
	if(use_safe_yaw){
		double diff_yaw = pseudo_yaw - base_yaw;
		hand_yaw += cos(pitch) * diff_yaw;
	}
#ifdef DEBUG
	printf("\tdiff_yaw: %lf\n",diff_yaw);
	printf("\thand_yaw: %lf\n",hand_yaw);
	printf("\tmod_yaw: %lf\n",hand_yaw);
	printf("\tpitch, cos(pitch): %lf %lf\n",pitch,cos(pitch));
#endif
  std::vector<double> output = get_xz_angles( xy_coord, dz, pitch );
  output[0] = base_yaw;
	output[4] = hand_yaw;
	return output;
}
std :: vector < double > YouBot_kinematics_inverse_arm ( Transform tr , std :: vector < double > & q , char & is_reach_back , bool use_safe_yaw = true ) { double dx , dy , dz , base_yaw , pseudo_yaw , pitch , hand_yaw , tmp1 , xy_coord ; char unique_pitch , yaw_issue ; tmp1 = tr ( 2 , 2 ) ; unique_pitch = tmp1 > .999 ? 1 : ( tmp1 < -.999 ? -1 : 0 ) ; if ( unique_pitch != 0 ) { pitch = unique_pitch > 0 ? 0 : PI ; pseudo_yaw = 0 ; hand_yaw = asin ( tr ( 1 , 0 ) ) ; #ifdef DEBUG printf ( " \n \n " , unique_pitch ) ; #endif } else { pitch = acos ( tmp1 ) ; pseudo_yaw = atan2 ( tr ( 1 , 2 ) , tr ( 0 , 2 ) ) ; hand_yaw = atan2 ( tr ( 2 , 1 ) , - tr ( 2 , 0 ) ) ; #ifdef DEBUG printf ( " \n \n " ) ; #endif } dx = tr ( 0 , 3 ) ; dy = tr ( 1 , 3 ) ; dz = tr ( 2 , 3 ) ; xy_coord = sqrt ( dx * dx + dy * dy ) ; if ( xy_coord < 1e-9 ) { base_yaw = q [ 0 ] ; } else { base_yaw = atan2 ( dy , dx ) ; if ( ( base_yaw - q [ 0 ] ) >= PI_ALMOST ) { #ifdef DEBUG printf ( " \t \n " ) ; #endif base_yaw -= PI ; xy_coord *= -1 ; is_reach_back = 1 ; } else if ( ( q [ 0 ] - base_yaw ) >= PI_ALMOST ) { #ifdef DEBUG printf ( " \t \n " ) ; #endif base_yaw += PI ; xy_coord *= -1 ; is_reach_back = -1 ; } else is_reach_back = 0 ; } #ifdef DEBUG printf ( " \t \n " , base_yaw ) ; printf ( " \t \n " , q [ 0 ] ) ; printf ( " \t \n " , is_reach_back ) ; printf ( " \t \n " , pitch ) ; printf ( " \t \n " , pseudo_yaw ) ; printf ( " \t \n " , hand_yaw ) ; #endif yaw_issue = 0 ; if ( ( base_yaw - pseudo_yaw ) >= PI_HALF ) { #ifdef DEBUG printf ( " \t \n " ) ; #endif pseudo_yaw += PI ; hand_yaw -= PI ; pitch *= -1 ; yaw_issue = 1 ; } else if ( ( pseudo_yaw - base_yaw ) >= PI_HALF ) { #ifdef DEBUG printf ( " \t \n " ) ; #endif pseudo_yaw -= PI ; hand_yaw -= PI ; pitch *= -1 ; yaw_issue = -1 ; } #ifdef DEBUG printf ( " \t \n " , pitch ) ; printf ( " \t \n " , base_yaw ) ; printf ( " \t \n " , pseudo_yaw ) ; printf ( " \t \n " , hand_yaw ) ; #endif if ( pitch < 0 && is_reach_back == 0 ) { pitch += PI_DOUBLE ; } if ( hand_yaw > PI ) hand_yaw -= PI_DOUBLE ; else if ( hand_yaw < - PI ) hand_yaw += PI_DOUBLE ; if ( use_safe_yaw ) { double diff_yaw = pseudo_yaw - base_yaw ; hand_yaw += cos ( pitch ) * diff_yaw ; } #ifdef DEBUG printf ( " \t \n " , diff_yaw ) ; printf ( " \t \n " , hand_yaw ) ; printf ( " \t \n " , hand_yaw ) ; printf ( " \t \n " , pitch , cos ( pitch ) ) ; #endif std :: vector < double > output = get_xz_angles ( xy_coord , dz , pitch ) ; output [ 0 ] = base_yaw ; output [ 4 ] = hand_yaw ; return output ; }
Inverse given a transform
Default uses the safe yaw
Inverse given a transform Default uses the safe yaw
// Grab the pitch // Check for singularity when the z1 and z2 yaws are parallel // Form the yaw of the hand // Grab the position // Check vertical singularity // Decide to reach back // End check if close to vertical singularity // Make into positive space for the pitch (above PI) // Also flip the hand_yaw...? // Optional safe yaw: // Grab the XZ plane angles