Robots/YouBot/YouBotKinematics.cpp
// TODO: also output a singularity, if near one
also output a singularity, if near one
also output a singularity if near one
Transform YouBot_kinematics_forward_arm(const double *q, char& is_singular) {
  Transform tr;
  tr.rotateZ(q[0])
    .translateX(baseLength) 
    .rotateY(q[1])
    .translateZ(lowerArmLength)
    .rotateY(q[2])
    .translateZ(upperArmLength)
    .rotateY(q[3])
    .translateZ(wristLength+handLength)
    .rotateZ(q[4]);
	if(fabs(tr(2,2)) > 0.99){
		is_singular = 1;
	} else {
		is_singular = 0;
	}
  return tr;
}
Transform YouBot_kinematics_forward_arm ( const double * q , char & is_singular ) { Transform tr ; tr . rotateZ ( q [ 0 ] ) . translateX ( baseLength ) . rotateY ( q [ 1 ] ) . translateZ ( lowerArmLength ) . rotateY ( q [ 2 ] ) . translateZ ( upperArmLength ) . rotateY ( q [ 3 ] ) . translateZ ( wristLength + handLength ) . rotateZ ( q [ 4 ] ) ; if ( fabs ( tr ( 2 , 2 ) ) > 0.99 ) { is_singular = 1 ; } else { is_singular = 0 ; } return tr ; }
TODO: also output a singularity, if near one
TODO : also output a singularity if near one
// do not use for now // Singularity checks