/**\fn void setStartXYZ(struct device *device0)
\brief set the starting desired xyz coordinate (pos_d = pos)
Set the initial desired position equal to the actual position so that
system starts without error.
\struct device
\param device0 pointer to device
\return
\ingroup Control
(could also be [ ]ingroup DataStructures )
*/
void setStartXYZ(struct device *device0)
{
int i;
static int j;
j++;
//Get the forward kinematics of this position
fwdCableCoupling(device0, RL_INIT);
r2_fwd_kin(device0, RL_INIT);
//Set XYZ offsets
for (i = 0; i < NUM_MECH; i++)
{
device0->mech[i].pos_d.x = device0->mech[i].pos.x;
device0->mech[i].pos_d.y = device0->mech[i].pos.y;
device0->mech[i].pos_d.z = device0->mech[i].pos.z;
device0->mech[i].ori_d.yaw = device0->mech[i].ori.yaw;
device0->mech[i].ori_d.pitch = device0->mech[i].ori.pitch;
device0->mech[i].ori_d.roll = device0->mech[i].ori.roll;
device0->mech[i].ori_d.grasp = device0->mech[i].ori.grasp;
for (int j=0;j<3;j++)
for (int k=0;k<3;k++)
device0->mech[i].ori_d.R[j][k] = device0->mech[i].ori.R[j][k];
}
// Update the origin, to which master-side deltas are added.
updateMasterRelativeOrigin( device0 );
}
/**
* \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;
}
