void RigidBody::operator()(const RigidBody::InternalState &x, RigidBody::InternalState &dxdt, const double /* t */)
{
Eigen::Vector3d x_dot, v_dot, omega_dot;
Eigen::Vector4d q_dot;
Eigen::Vector4d q(attitude.x(), attitude.y(), attitude.z(), attitude.w());
Eigen::Vector3d omega = angularVelocity;
Eigen::Matrix4d omegaMat = Omega(omega);
x_dot = velocity;
v_dot = force/mass;
q_dot = 0.5*omegaMat*q;
omega_dot = inertia.inverse() *
(torque - omega.cross(inertia*omega));
dxdt[0] = x_dot(0);
dxdt[1] = x_dot(1);
dxdt[2] = x_dot(2);
dxdt[3] = v_dot(0);
dxdt[4] = v_dot(1);
dxdt[5] = v_dot(2);
dxdt[6] = q_dot(0);
dxdt[7] = q_dot(1);
dxdt[8] = q_dot(2);
dxdt[9] = q_dot(3);
dxdt[10] = omega_dot(0);
dxdt[11] = omega_dot(1);
dxdt[12] = omega_dot(2);
}
void calc (JointData & data,
const Eigen::VectorXd & qs,
const Eigen::VectorXd & vs ) const
{
Eigen::VectorXd::ConstFixedSegmentReturnType<NQ>::Type & q = qs.segment<NQ> (idx_q ());
Eigen::VectorXd::ConstFixedSegmentReturnType<NV>::Type & q_dot = vs.segment<NQ> (idx_v ());
double c_theta,s_theta; SINCOS (q(2), &s_theta, &c_theta);
data.M.rotation ().topLeftCorner <2,2> () << c_theta, -s_theta, s_theta, c_theta;
data.M.translation ().head <2> () = q.head<2> ();
data.v.x_dot_ = q_dot(0);
data.v.y_dot_ = q_dot(1);
data.v.theta_dot_ = q_dot(2);
}
/*!
Halt all the axis.
*/
void
vpRobotBiclops::stopMotion(void)
{
vpColVector q_dot(vpBiclops::ndof);
q_dot = 0;
controller.setVelocity(q_dot);
//std::cout << "Request to stop the velocity controller thread...."<< std::endl;
controller.stopRequest(true);
}
void realrobot::jointCallback(const rt_dynamixel_msgs::JointStateConstPtr msg)
{
for(int i=0; i<total_dof; i++)
{
for (int j=0; j<msg->id.size(); j++)
{
if(jointID[i] == msg->id[j])
{
q(i) = msg->angle[j];
if(isFirstBoot)
{ _desired_q(i) = msg->angle[j]; }
q_dot(i) = msg->velocity[j];
torque(i) = msg->current[j];
jointStateUIPub.msg_.error[i] = msg->updated[j];
}
}
}
if(isFirstBoot)
{isFirstBoot = false;}
_jointRecv = true;
}
int main(int argc, char *argv[])
{
ros::init(argc, argv, "kinematics_publisher");
ros::NodeHandle nh;
ros::Publisher joint_publisher;
ros::Publisher frame_publisher;
joint_publisher = nh.advertise<sensor_msgs::JointState>("kinematics/joint_states", 1, true);
frame_publisher = nh.advertise<geometry_msgs::PoseStamped>("/frame_result", 1, true);
std::vector<std::string> names;
names.resize(2);
names.at(0) = "base_to_body";
names.at(1) = "body_to_head";
sensor_msgs::JointState my_joints;
my_joints.name = names;
my_joints.position.resize(2);
my_joints.position.at(0) = 0.2;
my_joints.position.at(1) = 0,2;
joint_publisher.publish(my_joints);
ros::spinOnce();
std::cin.get();
bool exit_value;
KDL::Tree my_tree;
urdf::Model my_model;
if (!kdl_parser::treeFromParam("/robot_description", my_tree)){
ROS_ERROR("Failed to construct kdl tree");
return false;
}
std::cout << "Number of links of the urdf:" << my_tree.getNrOfSegments() << std::endl;
std::cout << "Number of Joints of the urdf:" << my_tree.getNrOfJoints() << std::endl;
KDL::Chain my_chain;
my_tree.getChain("world", "webcam", my_chain);
std::cout << "Number of links of the CHAIN:" << my_chain.getNrOfSegments() << std::endl;
std::cout << "Number of Joints of the CHAIN:" << my_chain.getNrOfJoints() << std::endl;
KDL::ChainFkSolverPos_recursive fksolver(my_chain);
KDL::JntArray q(my_chain.getNrOfJoints());
q(0) = my_joints.position.at(0);
q(1) = my_joints.position.at(1);
KDL::Frame webcam;
fksolver.JntToCart(q,webcam);
geometry_msgs::Pose webcam_pose;
tf::poseKDLToMsg(webcam, webcam_pose);
geometry_msgs::PoseStamped webcam_stamped;
webcam_stamped.pose = webcam_pose;
webcam_stamped.header.frame_id = "world";
webcam_stamped.header.stamp = ros::Time::now();
frame_publisher.publish(webcam_stamped);
ros::spinOnce();
KDL::ChainJntToJacSolver jacobian_solver(my_chain);
KDL::Jacobian J;
J.resize(2);
jacobian_solver.JntToJac(q, J);
std::cout << J.data << std::endl;
webcam.M.DoRotY(-20*KDL::deg2rad);
tf::poseKDLToMsg(webcam, webcam_pose);
webcam_stamped.pose = webcam_pose;
webcam_stamped.header.stamp = ros::Time::now();
frame_publisher.publish(webcam_stamped);
ros::spinOnce();
std::cin.get();
KDL::ChainIkSolverVel_pinv ik_solver_vel(my_chain);
KDL::ChainIkSolverPos_NR ik_solver_pos(my_chain,fksolver,ik_solver_vel);
KDL::JntArray q_sol(2);
ik_solver_pos.CartToJnt(q, webcam,q_sol);
my_joints.position.at(0) = q_sol(0);
my_joints.position.at(1) = q_sol(1);
joint_publisher.publish(my_joints);
ros::spinOnce();
std::cin.get();
KDL::Vector g (0.0,0.0,-9.81);
KDL::ChainDynParam my_chain_params(my_chain, g);
KDL::ChainIdSolver_RNE id_solver(my_chain, g);
KDL::JntArray G(2);
my_chain_params.JntToGravity(q_sol,G);
KDL::JntSpaceInertiaMatrix H(2);
my_chain_params.JntToMass(q_sol, H);
KDL::JntArray C(2);
KDL::JntArray q_dot(2);
q_dot(0) = 0.02;
q_dot(1) = 0.04;
my_chain_params.JntToCoriolis(q_sol,q_dot, C);
std::cout << "g(q): " <<G.data << std::endl;
std::cout << "M(q): " <<H.data << std::endl;
std::cout << "C(q,q_dot): " <<C.data << std::endl;
KDL::JntArray q_dotdot(2);
q_dotdot(0) = -0.02;
q_dotdot(1) = 0.02;
KDL::Wrenches f(4);
KDL::JntArray tau(2);
id_solver.CartToJnt(q_sol, q_dot, q_dotdot, f, tau);
std::cout << "Tau: " <<tau.data << std::endl;
std::cin.get();
moveit::planning_interface::MoveGroup my_group("webcam_robot");
my_group.setNamedTarget("look_left");
my_group.move();
return false;
}
/*
Control loop to manage the biclops joint limits in speed control.
This control loop is running in a seperate thread in order to detect each 5
ms joint limits during the speed control. If a joint limit is detected the
axis should be halted.
\warning Velocity control mode is not exported from the top-level Biclops API
class provided by Traclabs. That means that there is no protection in this
mode to prevent an axis from striking its hard limit. In position mode,
Traclabs put soft limits in that keep any command from driving to a position
too close to the hard limits. In velocity mode this protection does not exist
in the current API.
\warning With the understanding that hitting the hard limits at full
speed/power can damage the unit, damage due to velocity mode commanding is
under user responsibility.
*/
void * vpRobotBiclops::vpRobotBiclopsSpeedControlLoop (void * arg)
{
vpRobotBiclopsController *controller = ( vpRobotBiclopsController * ) arg;
int iter = 0;
// PMDAxisControl *panAxis = controller->getPanAxis();
// PMDAxisControl *tiltAxis = controller->getTiltAxis();
vpRobotBiclopsController::shmType shm;
vpDEBUG_TRACE(10, "Start control loop");
vpColVector mes_q;
vpColVector mes_q_dot;
vpColVector softLimit(vpBiclops::ndof);
vpColVector q_dot(vpBiclops::ndof);
bool *new_q_dot = new bool [ vpBiclops::ndof ];
bool *change_dir = new bool [ vpBiclops::ndof ]; // change of direction
bool *force_halt = new bool [ vpBiclops::ndof ]; // force an axis to halt
bool *enable_limit = new bool [ vpBiclops::ndof ]; // enable soft limit
vpColVector prev_q_dot(vpBiclops::ndof); // previous desired speed
double secure = vpMath::rad(2); // add a security angle before joint limit
// Set the soft limits
softLimit[0] = vpBiclops::panJointLimit - secure;
softLimit[1] = vpBiclops::tiltJointLimit - secure;
vpDEBUG_TRACE(12, "soft limit pan: %f tilt: %f",
vpMath::deg(softLimit[0]),
vpMath::deg(softLimit[1]));
// Initilisation
vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller->readShm();
vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
prev_q_dot [i] = shm.q_dot[i];
new_q_dot [i] = false;
change_dir [i] = false;
force_halt [i] = false;
enable_limit[i] = true;
}
// Initialize actual position and velocity
mes_q = controller->getActualPosition();
mes_q_dot = controller->getActualVelocity();
vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller->readShm();
// Updates the shm
for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
shm.actual_q[i] = mes_q[i];
shm.actual_q_dot[i] = mes_q_dot[i];
}
// Update the actuals positions
controller->writeShm(shm);
vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
vpDEBUG_TRACE (11, "unlock mutex vpMeasure_mutex");
pthread_mutex_unlock(&vpMeasure_mutex); // A position is available
while (! controller->isStopRequested()) {
// Get actual position and velocity
mes_q = controller->getActualPosition();
mes_q_dot = controller->getActualVelocity();
vpDEBUG_TRACE (11, "Lock mutex vpShm_mutex");
pthread_mutex_lock(&vpShm_mutex);
shm = controller->readShm();
// Updates the shm
for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
shm.actual_q[i] = mes_q[i];
shm.actual_q_dot[i] = mes_q_dot[i];
}
vpDEBUG_TRACE(12, "mes pan: %f tilt: %f",
vpMath::deg(mes_q[0]),
vpMath::deg(mes_q[1]));
vpDEBUG_TRACE(13, "mes pan vel: %f tilt vel: %f",
vpMath::deg(mes_q_dot[0]),
vpMath::deg(mes_q_dot[1]));
vpDEBUG_TRACE(12, "desired q_dot : %f %f",
vpMath::deg(shm.q_dot[0]),
vpMath::deg(shm.q_dot[1]));
vpDEBUG_TRACE(13, "previous q_dot : %f %f",
vpMath::deg(prev_q_dot[0]),
vpMath::deg(prev_q_dot[1]));
for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
// test if joint limits are reached
if (mes_q[i] < -softLimit[i]) {
vpDEBUG_TRACE(12, "Axe %d in low joint limit", i);
shm.status[i] = vpRobotBiclopsController::STOP;
shm.jointLimit[i] = true;
}
else if (mes_q[i] > softLimit[i]) {
vpDEBUG_TRACE(12, "Axe %d in hight joint limit", i);
shm.status[i] = vpRobotBiclopsController::STOP;
shm.jointLimit[i] = true;
}
else {
shm.status[i] = vpRobotBiclopsController::SPEED;
shm.jointLimit[i] = false;
}
// Test if new a speed is demanded
//if (shm.q_dot[i] != prev_q_dot[i])
if (std::fabs(shm.q_dot[i] - prev_q_dot[i]) > std::fabs(vpMath::maximum(shm.q_dot[i],prev_q_dot[i]))*std::numeric_limits<double>::epsilon())
new_q_dot[i] = true;
else
new_q_dot[i] = false;
// Test if desired speed change of sign
if ((shm.q_dot[i] * prev_q_dot[i]) < 0.)
change_dir[i] = true;
else
change_dir[i] = false;
}
vpDEBUG_TRACE(13, "status : %d %d", shm.status[0], shm.status[1]);
vpDEBUG_TRACE(13, "joint : %d %d", shm.jointLimit[0], shm.jointLimit[1]);
vpDEBUG_TRACE(13, "new q_dot : %d %d", new_q_dot[0], new_q_dot[1]);
vpDEBUG_TRACE(13, "new dir : %d %d", change_dir[0], change_dir[1]);
vpDEBUG_TRACE(13, "force halt : %d %d", force_halt[0], force_halt[1]);
vpDEBUG_TRACE(13, "enable limit: %d %d", enable_limit[0], enable_limit[1]);
bool updateVelocity = false;
for (unsigned int i=0; i < vpBiclops::ndof; i ++) {
// Test if a new desired speed is to apply
if (new_q_dot[i]) {
// A new desired speed is to apply
if (shm.status[i] == vpRobotBiclopsController::STOP) {
// Axis in joint limit
if (change_dir[i] == false) {
// New desired speed without change of direction
// We go in the joint limit
if (enable_limit[i] == true) { // limit detection active
// We have to stop this axis
// Test if this axis was stopped before
if (force_halt[i] == false) {
q_dot[i] = 0.;
force_halt[i] = true; // indicate that it will be stopped
updateVelocity = true; // We have to send this new speed
}
}
else {
// We have to apply the desired speed to go away the joint
// Update the desired speed
q_dot[i] = shm.q_dot[i];
shm.status[i] = vpRobotBiclopsController::SPEED;
force_halt[i] = false;
updateVelocity = true; // We have to send this new speed
}
}
else {
// New desired speed and change of direction.
if (enable_limit[i] == true) { // limit detection active
// Update the desired speed to go away the joint limit
q_dot[i] = shm.q_dot[i];
shm.status[i] = vpRobotBiclopsController::SPEED;
force_halt[i] = false;
enable_limit[i] = false; // Disable joint limit detection
updateVelocity = true; // We have to send this new speed
}
else {
// We have to stop this axis
// Test if this axis was stopped before
if (force_halt[i] == false) {
q_dot[i] = 0.;
force_halt[i] = true; // indicate that it will be stopped
enable_limit[i] = true; // Joint limit detection must be active
updateVelocity = true; // We have to send this new speed
}
}
}
}
else {
// Axis not in joint limit
// Update the desired speed
q_dot[i] = shm.q_dot[i];
shm.status[i] = vpRobotBiclopsController::SPEED;
enable_limit[i] = true; // Joint limit detection must be active
updateVelocity = true; // We have to send this new speed
}
}
else {
// No change of the desired speed. We have to stop the robot in case of
// joint limit
if (shm.status[i] == vpRobotBiclopsController::STOP) {// axis limit
if (enable_limit[i] == true) { // limit detection active
// Test if this axis was stopped before
if (force_halt[i] == false) {
// We have to stop this axis
q_dot[i] = 0.;
force_halt[i] = true; // indicate that it will be stopped
updateVelocity = true; // We have to send this new speed
}
}
}
else {
// No need to stop the robot
enable_limit[i] = true; // Normal situation, activate limit detection
}
}
}
// Update the actuals positions
controller->writeShm(shm);
vpDEBUG_TRACE (11, "unlock mutex vpShm_mutex");
pthread_mutex_unlock(&vpShm_mutex);
if (updateVelocity) {
vpDEBUG_TRACE(12, "apply q_dot : %f %f",
vpMath::deg(q_dot[0]),
vpMath::deg(q_dot[1]));
// Apply the velocity
controller -> setVelocity( q_dot );
}
// Update the previous speed for next iteration
for (unsigned int i=0; i < vpBiclops::ndof; i ++)
prev_q_dot[i] = shm.q_dot[i];
vpDEBUG_TRACE(12, "iter: %d", iter);
//wait 5 ms
vpTime::wait(5.0);
// if (pthread_mutex_trylock(&vpEndThread_mutex) == 0) {
// vpDEBUG_TRACE (12, "Calling thread will end");
// vpDEBUG_TRACE (12, "Unlock mutex vpEndThread_mutex");
// std::cout << "Calling thread will end" << std::endl;
// std::cout << "Unlock mutex vpEndThread_mutex" << std::endl;
//
// pthread_mutex_unlock(&vpEndThread_mutex);
// break;
// }
iter ++;
}
controller->stopRequest(false);
// Stop the robot
vpDEBUG_TRACE(10, "End of the control thread: stop the robot");
q_dot = 0;
controller -> setVelocity( q_dot );
delete [] new_q_dot;
delete [] change_dir;
delete [] force_halt;
delete [] enable_limit;
vpDEBUG_TRACE(11, "unlock vpEndThread_mutex");
pthread_mutex_unlock(&vpEndThread_mutex);
vpDEBUG_TRACE (10, "Exit control thread ");
// pthread_exit(0);
return NULL;
}