/**
* \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;
}
/**
* \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;
}
void JtATIController::updateHook()
{
if(use_ft_sensor && port_ftdata.read(wrench_msg) != RTT::NewData)
return;
if(!updateState()) return;
jnt_trq_cmd.setZero();
if(0)
{
getMassMatrix(mass);
id_dyn_solver->JntToMass(jnt_pos_kdl,mass_kdl);
RTT::log(RTT::Warning) << "mass : \n"<<mass<<RTT::endlog();
RTT::log(RTT::Warning) << "mass_kdl : \n"<<mass_kdl.data<<RTT::endlog();
}
if(use_ft_sensor){
if(use_kdl){
// remove B(q,qdot)
jnt_vel_kdl.data.setZero();
// Get the Wrench in the last segment's frame
tf::wrenchMsgToKDL(wrench_msg.wrench,wrench_kdl);
f_ext_kdl.back() = wrench_kdl;
// Get The full dynamics with external forces
id_rne_solver->CartToJnt(jnt_pos_kdl,jnt_vel_kdl,jnt_acc_kdl,f_ext_kdl,jnt_trq_kdl);
// Get Joint Gravity torque
id_dyn_solver->JntToGravity(jnt_pos_kdl,gravity_kdl);
// remove G(q)
jnt_trq_cmd = jnt_trq_kdl.data - gravity_kdl.data;
RTT::log(RTT::Debug) << "Adding FT data : \n"<<jnt_trq_cmd.transpose()<<RTT::endlog();
}else{
// Get Jacobian with KRC
getJacobian(J_tip_base);
getCartesianPosition(cart_pos);
tf::poseMsgToKDL(cart_pos,tool_in_base_frame);
tf::poseMsgToEigen(cart_pos,tool_in_base_frame_eigen);
J_tip_base.changeBase(tool_in_base_frame.M);
RTT::log(RTT::Debug) << "J : \n"<<J_tip_base.data<<RTT::endlog();
// Get Jacobian with KDL
jnt_to_jac_solver->JntToJac(jnt_pos_kdl,J_kdl,kdl_chain.getNrOfSegments());
RTT::log(RTT::Debug) << "J_kdl : \n"<<J_kdl.data<<RTT::endlog();
tf::wrenchMsgToEigen(wrench_msg.wrench,wrench);
//tf::wrenchMsgToKDL(wrench_msg.wrench,wrench_kdl);
wrench_kdl = tool_in_base_frame.M * wrench_kdl;
///tf::wrenchKDLToEigen(wrench_kdl,wrench);
jnt_trq_cmd = J_tip_base.data.transpose() * wrench;
}
}
if(use_kdl_gravity)
{
getGravityTorque(jnt_grav);
id_dyn_solver->JntToGravity(jnt_pos_kdl,gravity_kdl);
jnt_trq_cmd += kg.asDiagonal() * gravity_kdl.data - jnt_grav;
RTT::log(RTT::Debug) << "Adding gravity : \n"<<(kg.asDiagonal() * gravity_kdl.data - jnt_grav).transpose()<<RTT::endlog();
}
if(compensate_coriolis)
{
id_dyn_solver->JntToCoriolis(jnt_pos_kdl,jnt_vel_kdl,coriolis_kdl);
jnt_trq_cmd -= coriolis_kdl.data.asDiagonal()*jnt_vel;
RTT::log(RTT::Debug) << "Removing Coriolis : \n"<<(coriolis_kdl.data.asDiagonal()*jnt_vel).transpose()<<RTT::endlog();
}
if(add_damping)
{
jnt_trq_cmd -= kd.asDiagonal() * jnt_vel;
RTT::log(RTT::Debug) << "Adding damping : \n"<<-(kd.asDiagonal() * jnt_vel).transpose()<<RTT::endlog();
}
RTT::log(RTT::Debug) << "jnt_trq_cmd : \n"<<jnt_trq_cmd.transpose()<<RTT::endlog();
sendJointTorque(jnt_trq_cmd);
}
