/**
* 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;
}
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);
}
}
}
/**
* 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;
}
/**
* \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;
}