/**
* \brief For debugging robot, apply a set torque command (tau_d) to a joint.
* \param device0 is robot_device struct defined in DS0.h
* \param currParams is param_pass struct defined in DS1.h
* \return 0
*
* This function: only run in runlevel 1.2
* 1. It checks the run level
* 2. loops over all the joints and mechanisim to set the torque value
* MAX_DOF_PER_MECH is 8 and is defined in DS0.h
* NUM_MECH is the number of mechanisim of the robot
*
*/
int applyTorque(struct device *device0, struct param_pass *currParams)
{
// Only run in runlevel 1.2
if ( ! (currParams->runlevel == RL_INIT && currParams->sublevel == SL_AUTO_INIT ))
return 0;
for (int i=0;i<NUM_MECH;i++)
{
for (int j=0;j<MAX_DOF_PER_MECH;j++)
{
if (device0->mech[i].type == GOLD_ARM)
{
device0->mech[i].joint[j].tau_d = (1.0/1000.0) * (float)(currParams->torque_vals[j]); // convert from mNm to Nm
}
else
{
device0->mech[i].joint[j].tau_d = (1.0/1000.0) * (float)(currParams->torque_vals[MAX_DOF_PER_MECH+j]);
}
}
}
// gravComp(device0);
TorqueToDAC(device0);
return 0;
}
/**
* \brief This function runs pd_control on motor position.
* \param device0 robot_device struct defined in DS0.h
* \param currParams param_pass struct defined in DS1.h
* \return -1 if Pedal is up and 0 when torque is applied to DAC
*
* This function:
* 1. calls the r2_inv_kin() to calculate the inverse kinematics
* 2. call the invCableCoupling() to calculate the inverse cable coupling
* 3. set all the joints to zero if pedal is not down otherwise it calls mpos_PD_control() to run the PD control law
* 4. calls getGravityTorque() to calulate gravity torques on each joints.
* 5. calls TorqueToDAC() to apply write torque value's on DAC
*
*/
int raven_cartesian_space_command(struct device *device0, struct param_pass *currParams){
struct DOF *_joint = NULL;
struct mechanism* _mech = NULL;
int i=0,j=0;
if (currParams->runlevel < RL_PEDAL_UP)
{
return -1;
}
else if (currParams->runlevel < RL_PEDAL_DN)
{
set_posd_to_pos(device0);
updateMasterRelativeOrigin(device0);
}
parport_out(0x01);
//Inverse kinematics
r2_inv_kin(device0, currParams->runlevel);
//Inverse Cable Coupling
invCableCoupling(device0, currParams->runlevel);
// Set all joints to zero torque
_mech = NULL; _joint = NULL;
while (loop_over_joints(device0, _mech, _joint, i,j) )
{
if (currParams->runlevel != RL_PEDAL_DN)
{
_joint->tau_d=0;
}
else
{
mpos_PD_control(_joint);
}
}
// Gravity compensation calculation
getGravityTorque(*device0, *currParams);
_mech = NULL; _joint = NULL;
while ( loop_over_joints(device0, _mech, _joint, i,j) )
{
_joint->tau_d += _joint->tau_g; // Add gravity torque
}
TorqueToDAC(device0);
return 0;
}
/**
* 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;
}
/**
* \brief Implements control for one loop cycle.
*
* \post encoders values have been read, runlevel has been set
* \pre robot state is reflected in device0, DAC outputs are set in device0
*
* \brief This funtion controls the Raven based on the desired control mode.
* \param device0 robot_device struct defined in DS0.h
* \param currParam param_pass struct defined in DS1.h
*
* This function first initializes the robot and then it computes Mpos and Velocities by calling
* stateEstimate() and then it calls fwdCableCoupling() and r2_fwd_kin() to calculate the forward cable coupling
* inverse kinematics respectively.
* The following types of control can be selected from the control mode:
* -No control
* -Cartesian Space Control
* -Motor PD Control
* -Joint Velocity Control
* -Homing mode
* -Applying arbitrary torque
*
*/
int controlRaven(struct device *device0, struct param_pass *currParams){
int ret = 0;
//Desired control mode
t_controlmode controlmode = (t_controlmode)currParams->robotControlMode;
//Initialization code
initRobotData(device0, currParams->runlevel, currParams);
//Compute Mpos & Velocities
stateEstimate(device0);
//Foward Cable Coupling
fwdCableCoupling(device0, currParams->runlevel);
//Forward kinematics
r2_fwd_kin(device0, currParams->runlevel);
switch (controlmode){
//CHECK ME: what is the purpose of this mode?
case no_control:
{
initialized = false;
struct DOF *_joint = NULL;
struct mechanism* _mech = NULL;
int i=0,j=0;
// Gravity compensation calculation
getGravityTorque(*device0, *currParams);
while ( loop_over_joints(device0, _mech, _joint, i,j) )
_joint->tau_d = _joint->tau_g; // Add gravity torque
TorqueToDAC(device0);
break;
}
//Cartesian Space Control is called to control the robot in cartesian space
case cartesian_space_control:
ret = raven_cartesian_space_command(device0,currParams);
break;
//Motor PD control runs PD control on motor position
case motor_pd_control:
initialized = false;
ret = raven_motor_position_control(device0,currParams);
break;
//Runs joint velocity control
case joint_velocity_control:
initialized = false;
ret = raven_joint_velocity_control(device0, currParams);
break;
//Runs homing mode
case homing_mode:
static int hom = 0;
if (hom==0){
log_msg("Entered homing mode");
hom = 1;
}
initialized = false;
//initialized = robot_ready(device0) ? true:false;
ret = raven_homing(device0, currParams);
set_posd_to_pos(device0);
updateMasterRelativeOrigin(device0);
if (robot_ready(device0))
{
currParams->robotControlMode = cartesian_space_control;
newRobotControlMode = cartesian_space_control;
}
break;
//Runs applyTorque() to set torque command (tau_d) to a joint for debugging purposes
case apply_arbitrary_torque:
initialized = false;
ret = applyTorque(device0, currParams);
break;
//Apply sinusoidal trajectory to all joints
case multi_dof_sinusoid:
initialized = false;
ret = raven_sinusoidal_joint_motion(device0, currParams);
break;
default:
ROS_ERROR("Error: unknown control mode in controlRaven (rt_raven.cpp)");
ret = -1;
break;
}
return ret;
}
/**
* \brief This function runs pi_control on joint velocity
* \param device0 is robot_device struct defined in DS0.h
* \param currParams is param_pass struct defined in DS1.h
* \return 0
*
* This function:
* 1. if pedal is down or RL_INIT and SL_AUTO_INIT, it Loops over all the joints and all the mechanisim to initialize
* velocity trajectory by calling start_trajectory() which is in trajectory.cpp
* 2. calls update_linear_sinusoid_velocity_trajectory() to get the desired joint velocities
* 3. calls jvel_PI_control() to run PI control, which is in pid_control.cpp
* 4. calls TorqueToDac() to apply torque on DAC
* It sets all the joint torques to zero if pedal is not down or not (RL_INIT and SL_AUTO_INIT)
*
*/
int raven_joint_velocity_control(struct device *device0, struct param_pass *currParams)
{
static int controlStart;
static unsigned long int delay=0;
// Run velocity control
if ( currParams->runlevel == RL_PEDAL_DN ||
( currParams->runlevel == RL_INIT &&
currParams->sublevel == SL_AUTO_INIT ))
{
// delay the start of control for 300ms b/c the amps have to turn on.
if (gTime - delay < 800)
return 0;
for (int i=0; i < NUM_MECH; i++)
{
for (int j = 0; j < MAX_DOF_PER_MECH; j++)
{
struct DOF * _joint = &(device0->mech[i].joint[j]);
if (device0->mech[i].type == GOLD_ARM)
{
// initialize velocity trajectory
if (!controlStart)
start_trajectory(_joint);
// Get the desired joint velocities
update_linear_sinusoid_velocity_trajectory(_joint);
// Run PI control
jvel_PI_control(_joint, !controlStart);
}
else
{
_joint->tau_d = 0;
}
}
}
if (!controlStart)
controlStart = 1;
// Convert joint torque to DAC value.
TorqueToDAC(device0);
}
else
{
delay=gTime;
controlStart = 0;
for (int i=0; i < NUM_MECH; i++)
for (int j = 0; j < MAX_DOF_PER_MECH; j++)
{
device0->mech[i].joint[j].tau_d=0;
}
TorqueToDAC(device0);
}
return 0;
}
/**\
* \brief This function runs PD control on motor position
* \param device0 is robot_device struct defined in DS0.h
* \param currParams is param_pass struct defined in DS1.h
* \return 0
*
* This function:
* 1. checks to see if it's in pedal down mode, if not it sets all joints to zero torque and return 0
* 2. set trajectory on all joints
* 3. calls invCableCoupling() to calculate inverse cable coupling
* 4. calls mpos_PD_control() to perform PD control
* 5. calls TorqueToDac() to apply torque on DAC
*/
int raven_motor_position_control(struct device *device0, struct param_pass *currParams)
{
static int controlStart = 0;
static unsigned long int delay=0;
struct DOF *_joint = NULL;
struct mechanism* _mech = NULL;
int i=0,j=0;
// If we're not in pedal down or init.init then do nothing.
if (! ( currParams->runlevel == RL_PEDAL_DN ||
( currParams->runlevel == RL_INIT && currParams->sublevel == SL_AUTO_INIT ))
)
{
controlStart = 0;
delay = gTime;
// Set all joints to zero torque, and mpos_d = mpos
_mech = NULL; _joint = NULL;
while (loop_over_joints(device0, _mech, _joint, i,j) )
{
_joint->mpos_d = _joint->mpos;
_joint->tau_d = 0;
}
return 0;
}
if (gTime - delay < 800)
return 0;
// Set trajectory on all the joints
_mech = NULL; _joint = NULL;
while (loop_over_joints(device0, _mech, _joint, i,j) )
{
if ( _joint->type == SHOULDER_GOLD || _joint->type == ELBOW_GOLD )
_joint->jpos_d = _joint->jpos;
if (!controlStart)
_joint->jpos_d = _joint->jpos;
}
//Inverse Cable Coupling
invCableCoupling(device0, currParams->runlevel);
// Do PD control on all the joints
_mech = NULL; _joint = NULL;
while (loop_over_joints(device0, _mech, _joint, i,j) )
{
// Do PD control
mpos_PD_control(_joint);
if (_joint->type < Z_INS_GOLD)
_joint->tau_d=0;
else if (gTime % 500 == 0 && _joint->type == Z_INS_GOLD)
log_msg("zp: %f, \t zp_d: %f, \t mp: %f, \t mp_d:%f", _joint->jpos, _joint->jpos_d, _joint->mpos, _joint->mpos_d);
}
TorqueToDAC(device0);
controlStart = 1;
return 0;
}
/**
* \brief This function applies a sinusoidal trajectory to all joints
* \param device0 is robot_device struct defined in DS0.h
* \param currParams is param_pass struct defined in DS1.h
* \return 0
*
* This function:
* 1. returns 0 if not in pedal down or init.init (do nothing)
* 2. it sets trajectory on all the joints
* 3. calls the invCableCoupling() to calculate inverse cable coupling
* 4. calls the mpos_PD_control() to run the PD control law
* 5. calls TorqueToDAC() to apply write torque value's on DAC
*
*/
int raven_sinusoidal_joint_motion(struct device *device0, struct param_pass *currParams){
static int controlStart = 0;
static unsigned long int delay=0;
// modify the period and magnitude / sep 15, xiao li
const float f_period[8] = {6, 7, 4, 9999999, 10, 5, 10, 6};
//const float f_period[8] = {6, 7, 10, 9999999, 5, 5, 10, 6};
// const float f_magnitude[8] = {10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999,
//30 DEG2RAD, 30 DEG2RAD, 30 DEG2RAD, 30 DEG2RAD};
const float f_magnitude[8] = {10 DEG2RAD, 10 DEG2RAD, 0.02, 9999999,
0 DEG2RAD, 0 DEG2RAD, 0 DEG2RAD, 0 DEG2RAD};
// If we're not in pedal down or init.init then do nothing.
if (! ( currParams->runlevel == RL_INIT && currParams->sublevel == SL_AUTO_INIT ))
{
controlStart = 0;
delay = gTime;
// Set all joints to zero torque, and mpos_d = mpos
for (int i=0; i < NUM_MECH; i++)
{
for (int j = 0; j < MAX_DOF_PER_MECH; j++)
{
struct DOF* _joint = &(device0->mech[i].joint[j]);
_joint->mpos_d = _joint->mpos;
_joint->jpos_d = _joint->jpos;
_joint->tau_d = 0;
}
}
return 0;
}
// Wait for amplifiers to power up
if (gTime - delay < 800)
return 0;
// Set trajectory on all the joints
for (int i=0; i < NUM_MECH; i++)
{
for (int j = 0; j < MAX_DOF_PER_MECH; j++)
{
struct DOF * _joint = &(device0->mech[i].joint[j]);
int sgn = 1;
if (device0->mech[i].type == GREEN_ARM)
sgn = -1;
// initialize trajectory
if (!controlStart)
start_trajectory(_joint, (_joint->jpos + sgn*f_magnitude[j]), f_period[j]);
// Get trajectory update
update_sinusoid_position_trajectory(_joint);
}
}
//Inverse Cable Coupling
invCableCoupling(device0, currParams->runlevel);
// Do PD control on all the joints
for (int i=0; i < NUM_MECH; i++)
{
for (int j = 0; j < MAX_DOF_PER_MECH; j++)
{
struct DOF * _joint = &(device0->mech[i].joint[j]);
// Do PD control
mpos_PD_control(_joint);
// if (is_toolDOF(_joint))
// _joint->tau_d = 0;
}
}
TorqueToDAC(device0);
controlStart = 1;
return 0;
}
