Ejemplo n.º 1
0
/**
*    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;
}
Ejemplo n.º 2
0
void stateEstimate(struct robot_device *device0)
{
    struct DOF *_joint;
    int i,j;

    //Loop through all joints
    for (i = 0; i < NUM_MECH; i++)
    {
        for (j = 0; j < MAX_DOF_PER_MECH; j++)
        {
            _joint = &(device0->mech[i].joint[j]);

//            if (_joint->type == TOOL_ROT_GOLD || _joint->type == TOOL_ROT_GOLD)
//                encToMPos(_joint);
//            else
                getStateLPF(_joint);

        }
    }
}
Ejemplo n.º 3
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;
}
Ejemplo n.º 4
0
/**
*   \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;
}