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