void lower_rarm(void) {
glColor3f(0.0, 1.0, 0.0);
glTranslatef(0.0, -0.880, 0.0);
glScalef(0.8, 0.75, 1.0);
drawCube();
right_hand();
}
/*! One possible formulation of the core GFO problem. Finds the contacts
forces that result in 0 object wrench and are as far as possible
from the edges of the friction cones. Assumes that at least one set
of contact forces that satisfy this criterion exist; see
contactForceExistence for that problem. See inner comments for exact
mathematical formulation. Not for standalone use; is called by the GFO
functions in the Grasp class.
*/
int
contactForceOptimization(Matrix &F, Matrix &N, Matrix &Q, Matrix &beta, double *objVal)
{
// exact problem formulation
// unknowns: beta (contact forces)
// minimize sum * F * beta (crude measure of friction resistance abilities)
// subject to:
// Q * beta = 0 (0 resultant object wrench)
// sum_normal * beta = 1 (we are applying some contact forces)
// F * beta <= 0 (each individual forces inside friction cones)
// beta >= 0 (all forces must be positive)
// overall equality constraint:
// | Q | |beta| |0|
// | N | = |1|
//right hand of equality
Matrix right_hand(Matrix::ZEROES<Matrix>(Q.rows()+1, 1));
//a total of 10N of normal force
right_hand.elem(Q.rows(),0) = 1.0e7;
//left hand of equality
Matrix LeftHand(Q.rows() + 1, Q.cols());
LeftHand.copySubMatrix(0, 0, Q);
LeftHand.copySubMatrix(Q.rows(), 0, N);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta.rows(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(beta.rows()));
//objective: sum of F
Matrix FSum(1,F.rows());
FSum.setAllElements(1.0);
Matrix FObj(1,F.cols());
matrixMultiply(FSum, F, FObj);
/*
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"Left Hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
F.print(fp);
fprintf(fp,"Objective:\n");
Q.print(fp);
fclose(fp);
*/
int result = LPSolver(FObj, LeftHand, right_hand, F, Matrix::ZEROES<Matrix>(F.rows(), 1),
lowerBounds, upperBounds,
beta, objVal);
return result;
}
void lower_right_arm(void) {
glTranslatef(-3.0, 0.0, 0.0);
glPushMatrix();
glScalef(5.0, 1.0, 1.0);
glutSolidCube(1.0);
glPopMatrix();
right_hand();
}
/*! One possible formulation of the core GFO problem. Checks if some
combination of contacts forces exists so that the resultant object
wrench is 0. See inner comments for exact mathematical formulation.
Not for standalone use; is called by the GFO functions in the
Grasp class.
*/
int
contactForceExistence(Matrix &F, Matrix &N, Matrix &Q, Matrix &beta, double *objVal)
{
// exact problem formulation
// unknowns: beta (contact forces)
// minimize betaT*QT*Q*beta (magnitude of resultant object wrench)
// subject to:
// sum_normal * beta = 1 (we are applying some contact forces)
// F * beta <= 0 (all forces inside friction cones)
// beta >= 0 (all forces must be positive)
Matrix right_hand(1,1);
//a total of 10N of normal force
right_hand.elem(0,0) = 1.0e7;
//right-hand side of inequality matrix
Matrix inEqZeroes(F.rows(), 1);
inEqZeroes.setAllElements(0.0);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta.rows(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(beta.rows()));
/*
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"N:\n");
N.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
F.print(fp);
fprintf(fp,"Objective:\n");
Q.print(fp);
fclose(fp);
*/
int result = factorizedQPSolver(Q, N, right_hand, F, inEqZeroes,
lowerBounds, upperBounds,
beta, objVal);
return result;
}
/*! One possible formulation of the core GFO problem. Checks if some
combination of joint forces exists so that the resultant object
wrench is 0. See inner comments for exact mathematical formulation.
Not for standalone use; is called by the GFO functions in the
Grasp class.
*/
int
graspForceExistence(Matrix &JTD, Matrix &D, Matrix &F, Matrix &G,
Matrix &beta, Matrix &tau, double *objVal)
{
// exact problem formulation
// unknowns: [beta tau] (contact forces and joint forces)
// minimize [beta tau]T*[G 0]T*[G 0]*[beta tau] (magnitude of resultant object wrench)
// subject to:
// [JTD -I] * [beta tau] = 0 (contact forces balance joint forces)
// [0 sum] * [beta tau] = 1 (we are applying some joint forces)
// [F 0] [beta tau] <= 0 (all forces inside friction cones)
// [beta tau] >= 0 (all forces must be positive)
// overall equality constraint:
// | JTD -I | | beta | |0|
// | 0 sum| | tau | = |1|
int numJoints = tau.rows();
Matrix beta_tau(beta.rows() + tau.rows(), 1);
//right-hand side of equality constraint
Matrix right_hand( JTD.rows() + 1, 1);
right_hand.setAllElements(0.0);
//actually, we use 1.0e9 here as units are in N * 1.0e-6 * mm
//so we want a total joint torque of 1000 N mm
right_hand.elem( right_hand.rows()-1, 0) = 1.0e10;
//left-hand side of equality constraint
Matrix LeftHand( JTD.rows() + 1, D.cols() + numJoints);
LeftHand.setAllElements(0.0);
LeftHand.copySubMatrix(0, 0, JTD);
LeftHand.copySubMatrix(0, D.cols(), Matrix::NEGEYE(numJoints, numJoints) );
for (int i=0; i<numJoints; i++) {
LeftHand.elem( JTD.rows(), D.cols() + i) = 1.0;
}
//matrix F padded with zeroes for tau
//left hand side of the inequality matrix
Matrix FO(F.rows(), F.cols() + numJoints);
FO.setAllElements(0.0);
FO.copySubMatrix(0, 0, F);
//right-hand side of inequality matrix
Matrix inEqZeroes(FO.rows(), 1);
inEqZeroes.setAllElements(0.0);
//objective matrix: G padded with zeroes
Matrix GO(Matrix::ZEROES<Matrix>(G.rows(), G.cols() + numJoints));
GO.copySubMatrix(0, 0, G);
//bounds: all variables greater than 0
// CHANGE: only beta >= 0, tau is not
Matrix lowerBounds(Matrix::MIN_VECTOR(beta_tau.rows()));
lowerBounds.copySubMatrix( 0, 0, Matrix::ZEROES<Matrix>(beta.rows(), 1) );
Matrix upperBounds(Matrix::MAX_VECTOR(beta_tau.rows()));
/*
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"left hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
FO.print(fp);
fprintf(fp,"Objective:\n");
GO.print(fp);
fclose(fp);
*/
// assembled system:
// minimize beta_tauT*QOT*QO*beta_tau subject to:
// LeftHand * beta_tau = right_hand
// FO * beta_tau <= inEqZeroes
// beta_tau >= 0
// CHANGE: only beta >= 0, tau is not
int result = factorizedQPSolver(GO, LeftHand, right_hand, FO, inEqZeroes,
lowerBounds, upperBounds,
beta_tau, objVal);
beta.copySubBlock(0, 0, beta.rows(), 1, beta_tau, 0, 0);
tau.copySubBlock(0, 0, tau.rows(), 1, beta_tau, beta.rows(), 0);
return result;
}
/*! One possible formulation of the core GFO problem. Finds the joint
forces that result in 0 object wrench such that contact forces are as
far as possible from the edges of the friction cones. Assumes that at
least one set of contact forces that satisfy this criterion exist; see
contactForceExistence for that problem. See inner comments for exact
mathematical formulation. Not for standalone use; is called by the GFO
functions in the Grasp class.
*/
int
graspForceOptimization(Matrix &JTD, Matrix &D, Matrix &F, Matrix &G,
Matrix &beta, Matrix &tau, double *objVal)
{
// exact problem formulation
// unknowns: [beta tau] (contact forces and joint forces)
// minimize [sum] [F 0] [beta tau] (sort of as far inside the friction cone as possible, not ideal)
// subject to:
// [JTD -I] * [beta tau] = 0 (contact forces balance joint forces)
// [G 0]* [beta tau] = 0 (0 resultant object wrench)
// [0 sum] * [beta tau] = 1 (we are applying some joint forces)
// [F 0] [beta tau] <= 0 (all forces inside friction cones)
// [beta tau] >= 0 (all forces must be positive)
// overall equality constraint:
// | JTD -I | | beta | |0|
// | G 0 | | tau | = |0|
// | 0 sum| |1|
Matrix beta_tau(beta.rows() + tau.rows(), 1);
int numJoints = tau.rows();
//right-hand side of equality constraint
Matrix right_hand( JTD.rows() + G.rows() + 1, 1);
right_hand.setAllElements(0.0);
//actually, we use 1.0e8 here as units are in N * 1.0e-6 * mm
//so we want a total joint torque of 100 N mm
right_hand.elem( right_hand.rows()-1, 0) = 1.0e10;
//left-hand side of equality constraint
Matrix LeftHand( JTD.rows() + G.rows() + 1, D.cols() + numJoints);
LeftHand.setAllElements(0.0);
LeftHand.copySubMatrix(0, 0, JTD);
LeftHand.copySubMatrix(0, D.cols(), Matrix::NEGEYE(numJoints, numJoints) );
LeftHand.copySubMatrix(JTD.rows(), 0, G);
for (int i=0; i<numJoints; i++) {
LeftHand.elem( JTD.rows() + G.rows(), D.cols() + i) = 1.0;
}
//objective matrix
//matrix F padded with zeroes for tau
//will also serve as the left hand side of the inequality matrix
Matrix FO(F.rows(), F.cols() + numJoints);
FO.setAllElements(0.0);
FO.copySubMatrix(0, 0, F);
//summing matrix and objective matrix
Matrix FSum(1, F.rows());
FSum.setAllElements(1.0);
Matrix FObj(1, FO.cols());
matrixMultiply(FSum, FO, FObj);
//bounds: all variables greater than 0
Matrix lowerBounds(Matrix::ZEROES<Matrix>(beta_tau.rows(),1));
Matrix upperBounds(Matrix::MAX_VECTOR(beta_tau.rows()));
//right-hand side of inequality matrix
Matrix inEqZeroes(FO.rows(), 1);
inEqZeroes.setAllElements(0.0);
FILE *fp = fopen("gfo.txt","w");
fprintf(fp,"left hand:\n");
LeftHand.print(fp);
fprintf(fp,"right hand:\n");
right_hand.print(fp);
fprintf(fp,"friction inequality:\n");
FO.print(fp);
fprintf(fp,"Objective:\n");
FObj.print(fp);
fclose(fp);
// assembled system:
// minimize FObj * beta_tau subject to:
// LeftHand * beta_tau = right_hand
// FO * beta_tau <= inEqZeroes
// beta_tau >= 0
int result = LPSolver(FObj, LeftHand, right_hand, FO, inEqZeroes,
lowerBounds, upperBounds,
beta_tau, objVal);
beta.copySubBlock(0, 0, beta.rows(), 1, beta_tau, 0, 0);
tau.copySubBlock(0, 0, tau.rows(), 1, beta_tau, beta.rows(), 0);
return result;
}
void processKinect(KinectController &kinect_controller)
{
XnUserID users[3];
XnUInt16 users_count = 3;
xn::UserGenerator& UserGenerator = kinect_controller.getUserGenerator();
xn::DepthGenerator& depthGenerator = kinect_controller.getDepthGenerator();
UserGenerator.GetUsers(users, users_count);
skeleton_markers::Skeleton g_skel;
for (int i = 0; i < users_count; ++i)
{
XnUserID user = users[i];
if (!UserGenerator.GetSkeletonCap().IsTracking(user)){
continue;
}
XnPoint3D com;
UserGenerator.GetCoM(user, com);
depthGenerator.ConvertRealWorldToProjective(1, &com, &com);
glVertex2f(com.X, com.Y);
// Calculate the distance between human and camera
float dist = com.Z /1000.0f;
geometry_msgs::Twist vel;
if(g_LeftHandPositionHistory.Speed()>150)
{
vel.linear.x = 0.00;
vel.angular.z = -0.08;
ROS_INFO("Robot is moving anticlockwise %.2f", g_LeftHandPositionHistory.Speed());
}
else if(g_RightHandPositionHistory.Speed()>150 && g_LeftHandPositionHistory.Speed()>150)
{
ROS_INFO("Robot is moving....");
ros::Duration(1.0).sleep(); // sleep for 1 second
ROS_INFO("Robot.....");
}
else if(g_RightHandPositionHistory.Speed()>150){
vel.linear.x = 0.00;
vel.angular.z = 0.08;
ROS_INFO("Robot is moving clockwise");
}
else{
//velocity command send to robot for follow human.
if(dist >=1.5)
{
ROS_INFO("I am following you");
vel.linear.x = 0.1;
vel.angular.z = 0.0;
}
else if(dist <=1.5 && dist >=1.0)
{
ROS_INFO("I am rounding ...");
vel.linear.x = 0.0;
vel.angular.z = 0.1;
}
else{
ROS_INFO(" You are too near to me");
vel.linear.x = -0.1;
vel.angular.z = 0.0;
}
}
handtrajectory(UserGenerator, depthGenerator, user, XN_SKEL_RIGHT_HAND, true);
handtrajectory(UserGenerator, depthGenerator, user, XN_SKEL_LEFT_HAND, true);
publishTransform(kinect_controller, user, XN_SKEL_HEAD, fixed_frame, "head", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_NECK, fixed_frame, "neck", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_TORSO, fixed_frame, "torso", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_SHOULDER, fixed_frame, "left_shoulder", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_ELBOW, fixed_frame, "left_elbow", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_HAND, fixed_frame, "left_hand", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_SHOULDER, fixed_frame, "right_shoulder", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_ELBOW, fixed_frame, "right_elbow", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_HAND, fixed_frame, "right_hand", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_HIP, fixed_frame, "left_hip", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_KNEE, fixed_frame, "left_knee", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_LEFT_FOOT, fixed_frame, "left_foot", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_HIP, fixed_frame, "right_hip", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_KNEE, fixed_frame, "right_knee", g_skel);
publishTransform(kinect_controller, user, XN_SKEL_RIGHT_FOOT, fixed_frame, "right_foot", g_skel);
// Input Joint Positions And Orientations from kinect
// Upper Joints positions
XnSkeletonJointPosition joint_position_head;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_HEAD, joint_position_head);
KDL::Vector head(joint_position_head.position.X, joint_position_head.position.Y, joint_position_head.position.Z);
XnSkeletonJointPosition joint_position_neck;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_NECK, joint_position_neck);
KDL::Vector neck(joint_position_neck.position.X, joint_position_neck.position.Y, joint_position_neck.position.Z);
XnSkeletonJointPosition joint_position_torso;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_TORSO, joint_position_torso);
KDL::Vector torso(joint_position_torso.position.X, joint_position_torso.position.Y, joint_position_torso.position.Z);
// Left Arm ****
XnSkeletonJointPosition joint_position_left_hand;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_HAND, joint_position_left_hand);
KDL::Vector left_hand(joint_position_left_hand.position.X, joint_position_left_hand.position.Y, joint_position_left_hand.position.Z);
XnSkeletonJointPosition joint_position_left_elbow;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_ELBOW, joint_position_left_elbow);
KDL::Vector left_elbow(joint_position_left_elbow.position.X, joint_position_left_elbow.position.Y, joint_position_left_elbow.position.Z);
XnSkeletonJointPosition joint_position_left_shoulder;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_SHOULDER, joint_position_left_shoulder);
KDL::Vector left_shoulder(joint_position_left_shoulder.position.X, joint_position_left_shoulder.position.Y, joint_position_left_shoulder.position.Z);
// Right Arm ****
XnSkeletonJointPosition joint_position_right_hand;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_HAND, joint_position_right_hand);
KDL::Vector right_hand(joint_position_right_hand.position.X, joint_position_right_hand.position.Y, joint_position_right_hand.position.Z);
XnSkeletonJointPosition joint_position_right_elbow;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_ELBOW, joint_position_right_elbow);
KDL::Vector right_elbow(joint_position_right_elbow.position.X, joint_position_right_elbow.position.Y, joint_position_right_elbow.position.Z);
XnSkeletonJointPosition joint_position_right_shoulder;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_SHOULDER, joint_position_right_shoulder);
KDL::Vector right_shoulder(joint_position_right_shoulder.position.X, joint_position_right_shoulder.position.Y, joint_position_right_shoulder.position.Z);
// Right Leg ****
XnSkeletonJointPosition joint_position_right_hip;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_HIP, joint_position_right_hip);
KDL::Vector right_hip(joint_position_right_hip.position.X, joint_position_right_hip.position.Y, joint_position_right_hip.position.Z);
XnSkeletonJointPosition joint_position_right_knee;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_KNEE, joint_position_right_knee);
KDL::Vector right_knee(joint_position_right_knee.position.X, joint_position_right_knee.position.Y, joint_position_right_knee.position.Z);
XnSkeletonJointPosition joint_position_right_foot;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_RIGHT_FOOT, joint_position_right_foot);
KDL::Vector right_foot(joint_position_right_foot.position.X, joint_position_right_foot.position.Y, joint_position_right_foot.position.Z);
// Left Leg ****
XnSkeletonJointPosition joint_position_left_hip;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_HIP, joint_position_left_hip);
KDL::Vector left_hip(joint_position_left_hip.position.X, joint_position_left_hip.position.Y, joint_position_left_hip.position.Z);
XnSkeletonJointPosition joint_position_left_knee;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_KNEE, joint_position_left_knee);
KDL::Vector left_knee(joint_position_left_knee.position.X, joint_position_left_knee.position.Y, joint_position_left_knee.position.Z);
XnSkeletonJointPosition joint_position_left_foot;
UserGenerator.GetSkeletonCap().GetSkeletonJointPosition(user, XN_SKEL_LEFT_FOOT, joint_position_left_foot);
KDL::Vector left_foot(joint_position_left_foot.position.X, joint_position_left_foot.position.Y, joint_position_left_foot.position.Z);
// Upper joints rotations
XnSkeletonJointOrientation joint_orientation_head;
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_HEAD, joint_orientation_head);
XnFloat* h = joint_orientation_head.orientation.elements;
KDL::Rotation head_rotation(h[0], h[1], h[2],
h[3], h[4], h[5],
h[6], h[7], h[8]);
double head_roll, head_pitch, head_yaw;
head_rotation.GetRPY(head_roll, head_pitch, head_yaw);
// head yaw
static double head_angle_yaw = 0;
if (joint_orientation_head.fConfidence >= 0.5)
{
head_angle_yaw = head_pitch;
}
// head pitch
static double head_angle_pitch = 0;
if (joint_orientation_head.fConfidence >= 0.5)
{
head_angle_pitch = head_roll;
}
XnSkeletonJointOrientation joint_orientation_neck;
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_NECK, joint_orientation_neck);
XnFloat* n = joint_orientation_neck.orientation.elements;
KDL::Rotation neck_rotation(n[0], n[1], n[2],
n[3], n[4], n[5],
n[6], n[7], n[8]);
double neck_roll, neck_pitch, neck_yaw;
neck_rotation.GetRPY(neck_roll, neck_pitch, neck_yaw);
XnSkeletonJointOrientation joint_orientation_torso;
UserGenerator.GetSkeletonCap().GetSkeletonJointOrientation(user, XN_SKEL_TORSO, joint_orientation_torso);
XnFloat* t = joint_orientation_torso.orientation.elements;
KDL::Rotation torso_rotation(t[0], t[1], t[2],
t[3], t[4], t[5],
t[6], t[7], t[8]);
double torso_roll, torso_pitch, torso_yaw;
torso_rotation.GetRPY(torso_roll, torso_pitch, torso_yaw);
// torso yaw
static double torso_angle_yaw = 0;
if (joint_orientation_torso.fConfidence >= 0.5)
{
torso_angle_yaw = torso_pitch;
}
// the kinect and robot are in different rotation spaces,
// Process and output joint rotations to the robot.
// ARMS for robot
// left elbow roll ****
KDL::Vector left_elbow_hand(left_hand - left_elbow);
KDL::Vector left_elbow_shoulder(left_shoulder - left_elbow);
left_elbow_hand.Normalize();
left_elbow_shoulder.Normalize();
static double left_elbow_angle_roll = 0;
if (joint_position_left_hand.fConfidence >= 0.5 &&
joint_position_left_elbow.fConfidence >= 0.5 &&
joint_position_left_shoulder.fConfidence >= 0.5)
{
left_elbow_angle_roll = acos(KDL::dot(left_elbow_hand, left_elbow_shoulder));
left_elbow_angle_roll = left_elbow_angle_roll - PI;
}
// right elbow roll ****
KDL::Vector right_elbow_hand(right_hand - right_elbow);
KDL::Vector right_elbow_shoulder(right_shoulder - right_elbow);
right_elbow_hand.Normalize();
right_elbow_shoulder.Normalize();
static double right_elbow_angle_roll = 0;
if (joint_position_right_hand.fConfidence >= 0.5 &&
joint_position_right_elbow.fConfidence >= 0.5 &&
joint_position_right_shoulder.fConfidence >= 0.5)
{
right_elbow_angle_roll = acos(KDL::dot(right_elbow_hand, right_elbow_shoulder));
right_elbow_angle_roll = -(right_elbow_angle_roll - PI);
}
// left shoulder roll ****
KDL::Vector left_shoulder_elbow(left_elbow - left_shoulder);
KDL::Vector left_shoulder_right_shoulder(right_shoulder - left_shoulder);
left_shoulder_elbow.Normalize();
left_shoulder_right_shoulder.Normalize();
static double left_shoulder_angle_roll = 0;
if (joint_position_right_shoulder.fConfidence >= 0.5 &&
joint_position_left_elbow.fConfidence >= 0.5 &&
joint_position_left_shoulder.fConfidence >= 0.5)
{
left_shoulder_angle_roll = acos(KDL::dot(left_shoulder_elbow, left_shoulder_right_shoulder));
left_shoulder_angle_roll = left_shoulder_angle_roll - HALFPI;
}
// right shoulder roll ****
KDL::Vector right_shoulder_elbow(right_elbow - right_shoulder);
KDL::Vector right_shoulder_left_shoulder(left_shoulder - right_shoulder);
right_shoulder_elbow.Normalize();
right_shoulder_left_shoulder.Normalize();
static double right_shoulder_angle_roll = 0;
if (joint_position_left_shoulder.fConfidence >= 0.5 &&
joint_position_right_elbow.fConfidence >= 0.5 &&
joint_position_right_shoulder.fConfidence >= 0.5)
{
right_shoulder_angle_roll = acos(KDL::dot(right_shoulder_elbow, right_shoulder_left_shoulder));
right_shoulder_angle_roll = -(right_shoulder_angle_roll - HALFPI);
}
// left shoulder pitch ****
static double left_shoulder_angle_pitch = 0;
if (joint_position_left_shoulder.fConfidence >= 0.5 &&
joint_position_left_elbow.fConfidence >= 0.5)
{
left_shoulder_angle_pitch = asin(left_shoulder_elbow.y());
left_shoulder_angle_pitch = left_shoulder_angle_pitch + HALFPI;
}
// right shoulder pitch ****
static double right_shoulder_angle_pitch = 0;
if (joint_position_right_shoulder.fConfidence >= 0.5)
{
right_shoulder_angle_pitch = asin(right_shoulder_elbow.y());
right_shoulder_angle_pitch = -(right_shoulder_angle_pitch + HALFPI);
}
// left shoulder yaw ****
static double left_shoulder_angle_yaw = 0;
if (joint_position_left_shoulder.fConfidence >= 0.5)
{
left_shoulder_angle_yaw = asin(left_elbow_hand.x());
}
// right shoulder yaw ****
static double right_shoulder_angle_yaw = 0;
if (joint_position_right_shoulder.fConfidence >= 0.5)
{
right_shoulder_angle_yaw = asin(right_elbow_hand.x());
right_shoulder_angle_yaw = -(right_shoulder_angle_yaw);
}
// Use head for counter balancing
static double hp_min = -0.1;
static double hp_max = 0.65;
// if this number is close to zero head should be hp_min
// if it is close to or above HALFPI then head should be hp_max
double arm_pitch_factor = (fabs(right_shoulder_angle_pitch) +
fabs(left_shoulder_angle_pitch)) / 2.0;
if (arm_pitch_factor >= HALFPI)
head_angle_pitch = 0.65;
else
head_angle_pitch = hp_min + ((arm_pitch_factor / HALFPI) * (hp_max - hp_min));
// head follows arm most sideways
double arm_roll_factor = (right_shoulder_angle_roll +
left_shoulder_angle_roll) / 2.0;
head_angle_yaw = arm_roll_factor * -1.1;
sensor_msgs::JointState js;
js.name.push_back("elbow_left_roll");
js.position.push_back(left_elbow_angle_roll);
js.velocity.push_back(10);
js.name.push_back("shoulder_left_roll");
js.position.push_back(left_shoulder_angle_roll);
js.velocity.push_back(10);
js.name.push_back("shoulder_left_pitch");
js.position.push_back(left_shoulder_angle_pitch);
js.velocity.push_back(10);
js.name.push_back("shoulder_left_yaw");
js.position.push_back(left_shoulder_angle_yaw);
js.velocity.push_back(10);
js.name.push_back("elbow_right_roll");
js.position.push_back(right_elbow_angle_roll);
js.velocity.push_back(10);
js.name.push_back("shoulder_right_roll");
js.position.push_back(right_shoulder_angle_roll);
js.velocity.push_back(10);
js.name.push_back("shoulder_right_pitch");
js.position.push_back(right_shoulder_angle_pitch);
js.velocity.push_back(10);
js.name.push_back("shoulder_right_yaw");
js.position.push_back(right_shoulder_angle_yaw);
js.velocity.push_back(10);
js.name.push_back("neck_yaw");
js.position.push_back(head_angle_yaw);
js.velocity.push_back(10);
js.name.push_back("neck_pitch");
js.position.push_back(head_angle_pitch);
js.velocity.push_back(10);
joint_states_pub_.publish(js);
/////
// Following is IK method
/////
KDL::Vector left_hand_torso = left_hand; // - torso;
KDL::Vector right_hand_torso = right_hand; // - torso;
double scale = 0.0003432;
left_hand_torso = scale * left_hand_torso;
right_hand_torso = scale * right_hand_torso;
geometry_msgs::Point p;
if (joint_position_left_hand.fConfidence >= 0.5)
{
p.x = -left_hand_torso.z();
p.y = -left_hand_torso.x();
p.z = left_hand_torso.y();
left_arm_destination_pub_.publish(p);
}
if (joint_position_right_hand.fConfidence >= 0.5)
{
p.x = -right_hand_torso.z();
p.y = -right_hand_torso.x();
p.z = right_hand_torso.y();
right_arm_destination_pub_.publish(p);
}
// The robot will receive and try to mirror my position
geometry_msgs::Point torso_destination;
torso_destination.x = -torso.z() / 1000.0;
torso_destination.y = -torso.x() / 1000.0;
//robot_destination.z = torso.y() / 1000.0;
torso_destination.z = torso_pitch;
torso_destination_pub_.publish(torso_destination);
g_skel.user_id = user;
g_skel.header.stamp = ros::Time::now();
g_skel.header.frame_id = fixed_frame;
skeleton_pub_.publish(g_skel);
cmdVelPub.publish(vel);
//Visualize the trajectory of right hand in RVIZ
visualization_msgs::Marker points;
points.header.frame_id = "/openni_depth_frame";
points.header.stamp = ros::Time::now();
points.ns = "trajectory_right";
points.type = visualization_msgs::Marker::POINTS;
// LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
points.scale.x = 0.03;
points.scale.y = 0.03;
points.color.r = 1.0;
points.color.a = 0.5;
XnPoint3D pt;
for (int k = 0; k < g_RightHandPositionHistory.Size(); ++k) {
g_RightHandPositionHistory.GetValueScreen (k, pt);
geometry_msgs::Point p;
p.x = pt.Z/1000;
p.y = pt.Y/1000;
p.z = pt.X/1000;
points.points.push_back(p);
}
marker_pub.publish(points);
//Visualize the trajectory of left hand in RVIZ
visualization_msgs::Marker point;
point.header.frame_id = "/openni_depth_frame";
point.header.stamp = ros::Time::now();
point.ns = "trajectory_left";
point.type = visualization_msgs::Marker::POINTS;
// LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width
point.scale.x = 0.03;
point.scale.y = 0.03;
point.color.b = 1.0;
point.color.a = 0.5;
XnPoint3D pt1;
for (int k = 0; k < g_LeftHandPositionHistory.Size(); ++k) {
g_LeftHandPositionHistory.GetValueScreen (k, pt1);
geometry_msgs::Point p;
p.x = pt1.Z/1000;
p.y = pt1.Y/1000;
p.z = pt1.X/1000;
point.points.push_back(p);
}
marker_pub.publish(point);
break; // only read first user
}
}