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
}
}
// Main function
int main( int argc, char* argv[] )
{
ros::init( argc, argv, "nao_arm_control_node" );
ros::NodeHandle nh;
ros::Rate loop_rate(10);
tl = new tf::TransformListener();
global_frame = "openni_depth_frame";
suffix = "_1";
// Publisher
ros::Publisher joint_pub_ = nh.advertise<naoqi_msgs::JointAnglesWithSpeed>("joint_angles",10);
// Subscriber
ros::Subscriber tactileSub = nh.subscribe("nao_robot/contact/tactile_touch", 1, tactileCallBack);
// Enable stifness
ros::ServiceClient m_stiffnessDisableClient;
ros::ServiceClient m_stiffnessEnableClient;
m_stiffnessDisableClient = nh.serviceClient<std_srvs::Empty>("body_stiffness/disable");
m_stiffnessEnableClient = nh.serviceClient<std_srvs::Empty>("body_stiffness/enable");
std_srvs::Empty e;
m_stiffnessEnableClient.call(e);
while( ros::ok() )
{
// If the user has not perform any pose
if( !get_poses() )
{
loop_rate.sleep();
ros::spinOnce();
continue;
}
/***************************** ELBOW ANGLES *********************************/
// Calculate left elbow roll angle
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;
{
left_elbow_angle_roll = acos(KDL::dot(left_elbow_shoulder,left_elbow_hand));
left_elbow_angle_roll = left_elbow_angle_roll - PI;
//ROS_INFO("left_elbow_angle_roll: %.2f", left_elbow_angle_roll);
}
// Calculate right elbow roll angle
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;
{
right_elbow_angle_roll = acos(KDL::dot(right_elbow_hand, right_elbow_shoulder));
right_elbow_angle_roll = -(right_elbow_angle_roll - PI);
//ROS_INFO("right_elbow_angle_roll: %.2f", right_elbow_angle_roll);
}
// Calculate right elbow yaw angle
static double right_elbow_angle_yaw = 0;
right_elbow_angle_yaw = (right_elbow_hand[2] / sin(right_elbow_angle_roll)) * HALFPI;
if (right_elbow_angle_yaw > 1.5) right_elbow_angle_yaw = 1.5;
else if(right_elbow_angle_yaw < -1.5) right_elbow_angle_yaw = -1.5;
// Calculate left elbow yaw angle
static double left_elbow_angle_yaw = 0;
left_elbow_angle_yaw = (left_elbow_hand[2] / sin(left_elbow_angle_roll)) * HALFPI;
if (left_elbow_angle_yaw > 1.5) left_elbow_angle_yaw = 1.5;
else if(left_elbow_angle_yaw < -1.5) left_elbow_angle_yaw = -1.5;
//ROS_INFO("YAW:%g, Y:%g",right_elbow_angle_yaw,right_elbow_hand[2]);
/***************************** SHOULDER ANGLES *********************************/
// Calculate left shoulder roll angle
KDL::Vector left_shoulder_elbow(left_elbow - left_shoulder);
KDL::Vector left_shoulder_neck(right_shoulder - left_shoulder);
left_shoulder_elbow.Normalize();
left_shoulder_neck.Normalize();
static double left_shoulder_angle_roll = 0;
{
left_shoulder_angle_roll = acos(KDL::dot(left_shoulder_elbow, left_shoulder_neck));
left_shoulder_angle_roll = left_shoulder_angle_roll - HALFPI;
//ROS_INFO("left_shoulder_angle_roll: %f", left_shoulder_angle_roll);
}
// Calculate right shoulder roll angle
KDL::Vector right_shoulder_elbow(right_elbow - right_shoulder);
KDL::Vector right_shoulder_neck(left_shoulder - right_shoulder);
right_shoulder_elbow.Normalize();
right_shoulder_neck.Normalize();
static double right_shoulder_angle_roll = 0;
{
right_shoulder_angle_roll = acos(KDL::dot(right_shoulder_elbow, right_shoulder_neck));
right_shoulder_angle_roll = -(right_shoulder_angle_roll - HALFPI);
//ROS_INFO("right_shoulder_angle_roll: %f", right_shoulder_angle_roll);
}
// Calculate left shoulder pitch angle
static double left_shoulder_angle_pitch = 0;
{
left_shoulder_angle_pitch = asin(left_shoulder_elbow.z());
left_shoulder_angle_pitch = -(left_shoulder_angle_pitch);
//ROS_INFO("left_shoulder_angle_pitch: %f", left_shoulder_angle_pitch);
}
// Calculate right shoulder pitch angle
static double right_shoulder_angle_pitch = 0;
//if (joint_position_right_shoulder.fConfidence >= 0.5)
{
right_shoulder_angle_pitch = asin(right_shoulder_elbow.z());
right_shoulder_angle_pitch = -(right_shoulder_angle_pitch);
//ROS_INFO("left_shoulder_angle_pitch: %f", left_shoulder_angle_pitch);
}
// Correct some postures
// When the user puts down his/her hands
//ROS_INFO("Hand X:%g, Y:%g, Z:%g", right_hand[0], right_hand[1], right_hand[2]);
if(right_hand[2]<0) right_elbow_angle_yaw = 0;
if(left_hand[2]<0) left_elbow_angle_yaw = 0;
// Publish NAO's arm joint angles
naoqi_msgs::JointAnglesWithSpeed output;
output.joint_names.push_back("LElbowRoll");
output.joint_angles.push_back(left_elbow_angle_roll);
output.joint_names.push_back("RElbowRoll");
output.joint_angles.push_back(right_elbow_angle_roll);
output.joint_names.push_back("LElbowYaw");
output.joint_angles.push_back(left_elbow_angle_yaw);
output.joint_names.push_back("RElbowYaw");
output.joint_angles.push_back(right_elbow_angle_yaw);
output.joint_names.push_back("LShoulderRoll");
output.joint_angles.push_back(left_shoulder_angle_roll);
output.joint_names.push_back("RShoulderRoll");
output.joint_angles.push_back(right_shoulder_angle_roll);
output.joint_names.push_back("LShoulderPitch");
output.joint_angles.push_back(left_shoulder_angle_pitch);
output.joint_names.push_back("RShoulderPitch");
output.joint_angles.push_back(right_shoulder_angle_pitch);
// Set joint movement speed
output.speed = 0.3;
/*** TODO ***/
// Hand yaw angle
joint_pub_.publish(output);
ros::spinOnce();
loop_rate.sleep();
if(exited == true) {
std_srvs::Empty e;
m_stiffnessDisableClient.call(e);
break;
}
}
ROS_INFO("Killing nao_arm_control...");
ros::shutdown();
return 0;
}
