/** * set_joints_known_pos() * * Set all the mechanism joints to known reference angles. * Propogate the joint angle to motor position and encoder offset. */ int set_joints_known_pos(struct mechanism* _mech, int tool_only) { struct DOF* _joint=NULL; int j=0; /// Set joint position reference for just tools, or all DOFS _joint = NULL; while ( loop_over_joints(_mech, _joint ,j) ) { if ( tool_only && ! is_toolDOF( _joint->type)) { // Set jpos_d to the joint limit value. _joint->jpos_d = DOF_types[ _joint->type ].home_position; } else if (!tool_only && is_toolDOF(_joint->type) ) { _joint->jpos_d = _joint->jpos; } else { // Set jpos_d to the joint limit value. _joint->jpos_d = DOF_types[ _joint->type ].max_position; // Initialize a trajectory to operating angle _joint->state = jstate_homing1; } } /// Inverse cable coupling: jpos_d ---> mpos_d invMechCableCoupling(_mech, 1); _joint = NULL; while ( loop_over_joints(_mech, _joint ,j) ) { // Reset the state-estimate filter _joint->mpos = _joint->mpos_d; resetFilter( _joint ); // Convert the motor position to an encoder offset. // mpos = k * (enc_val - enc_offset) ---> enc_offset = enc_val - mpos/k float f_enc_val = _joint->enc_val; // Encoder values on Gold arm are reversed. See also state_machine.cpp if ( _mech->type == GOLD_ARM) f_enc_val *= -1.0; /// Set the joint offset in encoder space. float cc = ENC_CNTS_PER_REV / (2*M_PI); _joint->enc_offset = f_enc_val - (_joint->mpos_d * cc); getStateLPF(_joint); } fwdMechCableCoupling(_mech); return 0; }
/** * jointVelControl() * Move joints at constant rate. */ float jvel_PI_control(struct DOF *_joint, int resetI){ // Set gains. Gains have been "empirically" tuned. // TODO: move this to a permanent place. float ki; float kv[MAX_MECH * MAX_DOF_PER_MECH] = {0}; kv[SHOULDER_GOLD] = kv[SHOULDER_GREEN] = (0.528/(15 DEG2RAD)); kv[ELBOW_GOLD] = kv[ELBOW_GREEN] = (0.528/(15 DEG2RAD)); kv[Z_INS_GOLD] = kv[Z_INS_GREEN] = (0.400/0.1); kv[TOOL_ROT_GOLD] = kv[TOOL_ROT_GREEN] = (0.005/(15 DEG2RAD)); kv[WRIST_GOLD] = kv[WRIST_GREEN] = 0; kv[GRASP1_GOLD] = kv[GRASP1_GREEN] = 0; kv[GRASP2_GREEN] = kv[GRASP2_GREEN] = 0; static float jVelIntErr[MAX_MECH * MAX_DOF_PER_MECH]; // Integral of velocity error // Reset integral term if (resetI){ jVelIntErr[_joint->type] = 0; return 0; } float jVelErr; // Calculate joint velocity error if ( is_toolDOF(_joint) ) jVelErr = _joint->mvel_d - _joint->mvel; else jVelErr = _joint->jvel_d - _joint->jvel; // Integrate error over 1ms jVelIntErr[_joint->type] += jVelErr * ONE_MS; ki = kv[_joint->type] * 0.1 * 0; // Calculate PI velocity control _joint->tau_d = ( kv[_joint->type]*jVelErr + ki*jVelIntErr[_joint->type] ); return jVelIntErr[_joint->type]; }
/** * raven_homing() * * This function is called 1000 times per second during INIT mode * * \param device0 Which (top level) device to home (usually only one device per system) * \param currParams Current parameters (for Run Level) * \param begin_homing Flag to start the homing process * \ingroup Control * * \brief Move to hard stops in controled way, "zero" the joint value, and then move to "home" position * * This function operates in two phases: * -# Discover joint position by running to hard stop. Using PD control (I term zero'd) we move the joint at a smooth rate until * current increases which indicates hitting hard mechanical stop. * -# Move joints to "home" position. In this phase the robot moves from the joint limits to a designated pose in the center of the workspace. * \todo Homing limits should be Amps not DAC units (see homing() ). * \todo Eliminate or comment out Enders Game code!! */ int raven_homing(struct device *device0, struct param_pass *currParams, int begin_homing) { static int homing_inited = 0; static unsigned long int delay, delay2; struct DOF *_joint = NULL; struct mechanism* _mech = NULL; int i=0,j=0; // Only run in init mode if ( ! (currParams->runlevel == RL_INIT && currParams->sublevel == SL_AUTO_INIT )) { homing_inited = 0; delay = gTime; return 0; // return if we are in the WRONG run level (PLC state) } // Wait a short time for amps to turn on if (gTime - delay < 1000) { return 0; } // Initialize the homing sequence. if (begin_homing || !homing_inited) // only do this the first time through { // Zero out joint torques, and control inputs. Set joint.state=not_ready. _mech = NULL; _joint = NULL; while (loop_over_joints(device0, _mech, _joint, i,j) ) // foreach (joint) { _joint->tau_d = 0; _joint->mpos_d = _joint->mpos; _joint->jpos_d = _joint->jpos; _joint->jvel_d = 0; _joint->state = jstate_not_ready; if (is_toolDOF(_joint)) jvel_PI_control(_joint, 1); // reset PI control integral term homing_inited = 1; } log_msg("Homing sequence initialized"); } // Specify motion commands _mech = NULL; _joint = NULL; while ( loop_over_joints(device0, _mech, _joint, i,j) ) { // Initialize tools first. if ( is_toolDOF(_joint) || tools_ready( &(device0->mech[i]) ) ) { homing(_joint); } } //Inverse Cable Coupling invCableCoupling(device0, currParams->runlevel); // Do PD control on all joints _mech = NULL; _joint = NULL; while ( loop_over_joints(device0, _mech, _joint, i,j) ) { mpos_PD_control( _joint ); } // Calculate output DAC values TorqueToDAC(device0); // Check homing conditions and set joint angles appropriately. _mech = NULL; _joint = NULL; while ( loop_over_joints(device0, _mech, _joint, i,j) ) { struct DOF * _joint = &(_mech->joint[j]); ///\todo is this line necessary? // Check to see if we've reached the joint limit. if( check_homing_condition(_joint) ) { log_msg("Found limit on joint %d cmd: %d \t", _joint->type, _joint->current_cmd, DOF_types[_joint->type].DAC_max); _joint->state = jstate_hard_stop; _joint->current_cmd = 0; stop_trajectory(_joint); log_msg("joint %d checked ",j); } // For each mechanism, check to see if the mech is finished homing. if ( j == (MAX_DOF_PER_MECH-1) ) { /// if we're homing tools, wait for tools to be finished if (( !tools_ready(_mech) && _mech->joint[TOOL_ROT].state==jstate_hard_stop && _mech->joint[WRIST ].state==jstate_hard_stop && _mech->joint[GRASP1 ].state==jstate_hard_stop ) || ( tools_ready( _mech ) && _mech->joint[SHOULDER].state==jstate_hard_stop && _mech->joint[ELBOW ].state==jstate_hard_stop && _mech->joint[Z_INS ].state==jstate_hard_stop )) { if (delay2==0) delay2=gTime; if (gTime > delay2 + 200) // wait 200 ticks for cables to settle down { set_joints_known_pos(_mech, !tools_ready(_mech) ); // perform second phase delay2 = 0; } } } } return 0; }
/** * set_joints_known_pos() * * \param _mech which mechanism (gold/green) * \param tool_only flag which initializes only the tool/wrist joints * * \brief Set joint angles to known values after hard stops are reached. * * Set all the mechanism joints to known reference angles. * Propagate the joint angle to motor position and encoder offset. * * \todo Rationalize the sign changes on GREEN_ARM vs GOLD_ARM (see IFDEF below). * \todo This MAYBE needs to be changed to support device specific parameter changes read from a config file or ROS service. * \ingroup Control */ int set_joints_known_pos(struct mechanism* _mech, int tool_only) { struct DOF* _joint=NULL; int j=0; // int offset = 0; // if (_mech->type == GREEN_ARM) offset = 8; /// Set joint position reference for just tools, or all DOFS _joint = NULL; while ( loop_over_joints(_mech, _joint ,j) ) { // when tool joints finish, set positioning joints to neutral if ( tool_only && ! is_toolDOF( _joint->type)) { // Set jpos_d to the joint limit value. _joint->jpos_d = DOF_types[ _joint->type ].home_position; // keep non tool joints from moving } // when positioning joints finish, set tool joints to nothing special else if (!tool_only && is_toolDOF(_joint->type) ) { _joint->jpos_d = _joint->jpos; } // when tool or positioning joints finish, set them to max_angle else { // Set jpos_d to the joint limit value. _joint->jpos_d = DOF_types[ _joint->type ].max_position; // Initialize a trajectory to operating angle _joint->state = jstate_homing1; } } /// Inverse cable coupling: jpos_d ---> mpos_d // use_actual flag triggered for insertion axis invMechCableCoupling(_mech, 1); _joint = NULL; while ( loop_over_joints(_mech, _joint ,j) ) { // Reset the state-estimate filter _joint->mpos = _joint->mpos_d; resetFilter( _joint ); // Convert the motor position to an encoder offset. // mpos = k * (enc_val - enc_offset) ---> enc_offset = enc_val - mpos/k float f_enc_val = _joint->enc_val; // Encoder values on Gold arm are reversed. See also state_machine.cpp switch (_mech->tool_type) { case dv_adapter: if ( _mech->type == GOLD_ARM && !is_toolDOF(_joint)) f_enc_val *= -1.0; break; case RII_square_type: //Green arm tools are also reversed with square pattern if (( _mech->type == GOLD_ARM && !is_toolDOF(_joint) ) || ( _mech->type == GREEN_ARM && is_toolDOF(_joint) )) f_enc_val *= -1.0; break; default: if ( _mech->type == GOLD_ARM || is_toolDOF(_joint) ) f_enc_val *= -1.0; break; //Needs a true default/error state } /// Set the joint offset in encoder space. float cc = ENC_CNTS_PER_REV / (2*M_PI); _joint->enc_offset = f_enc_val - (_joint->mpos_d * cc); getStateLPF(_joint, _mech->tool_type); } fwdMechCableCoupling(_mech); return 0; }
int is_toolDOF(struct DOF *_joint){ return is_toolDOF(_joint->type); }
/** * \fn int set_joints_known_pos(struct mechanism* _mech, int tool_only) * * \brief Set joint angles to known values after hard stops are reached. * * \desc Set all the mechanism joints to known reference angles. * Propagate the joint angle to motor position and encoder offset. * * \param _mech which mechanism (gold/green) * \param tool_only flag which initializes only the tool/wrist joints * * \ingroup Control * * \return 0 * * \todo Rationalize the sign changes on GREEN_ARM vs GOLD_ARM (see IFDEF below). * \todo This MAYBE needs to be changed to support device specific parameter changes read from a config file or ROS service. */ int set_joints_known_pos(struct mechanism* _mech, int tool_only) { struct DOF* _joint=NULL; int j=0; int scissor = ((_mech->mech_tool.t_end == mopocu_scissor) || (_mech->mech_tool.t_end == potts_scissor))? 1 : 0; // int offset = 0; // if (_mech->type == GREEN_ARM) offset = 8; /// Set joint position reference for just tools, or all DOFS _joint = NULL; while ( loop_over_joints(_mech, _joint ,j) ) { // when tool joints finish, set positioning joints to neutral if ( tool_only && ! is_toolDOF( _joint->type)) { // Set jpos_d to the joint limit value. _joint->jpos_d = DOF_types[ _joint->type ].home_position; // keep non tool joints from moving } // when positioning joints finish, set tool joints to nothing special else if (!tool_only && is_toolDOF(_joint->type) ) { _joint->jpos_d = _joint->jpos; } // when tool or positioning joints finish, set them to max_angle else { // Set jpos_d to the joint limit value. if (scissor && ((_joint->type == GRASP2_GREEN) || (_joint->type == GRASP2_GOLD))){ _joint->jpos_d = _mech->mech_tool.grasp2_min_angle; log_msg("setting grasp 2 to arm %d, joint %d to value %f", _mech->mech_tool.mech_type, _joint->type, _joint->jpos_d); } else _joint->jpos_d = DOF_types[_joint->type].max_position; // Initialize a trajectory to operating angle _joint->state = jstate_homing1; } } /// Inverse cable coupling: jpos_d ---> mpos_d // use_actual flag triggered for insertion axis invMechCableCoupling(_mech, 1); _joint = NULL; while ( loop_over_joints(_mech, _joint ,j) ) { // Reset the state-estimate filter _joint->mpos = _joint->mpos_d; resetFilter( _joint ); // Convert the motor position to an encoder offset. // mpos = k * (enc_val - enc_offset) ---> enc_offset = enc_val - mpos/k float f_enc_val = _joint->enc_val; // Encoder values on Gold arm are reversed. See also state_machine.cpp switch (_mech->mech_tool.t_style){ case dv: if ( _mech->type == GOLD_ARM && !is_toolDOF(_joint)) f_enc_val *= -1.0; break; case square_raven: if ( ( _mech->type == GOLD_ARM && !is_toolDOF(_joint) ) || ( _mech->type == GREEN_ARM && is_toolDOF(_joint) ) //Green arm tools are also reversed with square pattern ) f_enc_val *= -1.0; break; default: if ( _mech->type == GOLD_ARM || is_toolDOF(_joint) ) f_enc_val *= -1.0; break; } #ifdef OPPOSE_GRIP if (j == GRASP1){ f_enc_val *= -1;// switch encoder value for opposed grasp // static int twice = 0; // if (twice < 2){ // log_msg("homing enc_val swapped"); // twice++; } #endif /// Set the joint offset in encoder space. float cc = ENC_CNTS_PER_REV / (2*M_PI); _joint->enc_offset = f_enc_val - (_joint->mpos_d * cc); getStateLPF(_joint, _mech->tool_type); //getStateLPF(_joint, _mech->mech_tool.t_style); } fwdMechCableCoupling(_mech); return 0; }