void left_leg(){
	glPushMatrix();
	glColor3f(0.0, 1.0, 0.0);
	glTranslatef(-0.015, -0.85, 0.0);
	glScalef(0.5, 0.7, 0.0);

	drawCube();
	left_foot();
	glPopMatrix();
}
Ejemplo n.º 2
0
void lower_left_leg(void){
	
	glTranslatef(0.0, -3.0, 0.0);
	
	glPushMatrix();
	
	glScalef(0.8, 4.5, 1.0);
	glutSolidCube(1.0);
	
	glPopMatrix();
	
	
	left_foot();
	
}
void OddishModel::drawCyborg(GLuint texture1,GLuint texture2,GLuint texture3){
	glOrtho(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0);
	glRotatef(thetaO[0], 0.0, 1.0, 0.0);
	glPushMatrix();
	glTranslatef(0,TORSO_HEIGHT_ODDISH-1.0, 0.0);
	torso(texture2);
	glPopMatrix();
	glPushMatrix();

	glTranslatef(0,TORSO_HEIGHT_ODDISH, 0.0);

	glTranslatef(0.0, NECK_HEIGHT_ODDISH+0.5*HEAD_HEIGHT_ODDISH-0.3, 0.0);
	head(texture1);

	glTranslatef(-0.70, NECK_HEIGHT_ODDISH+0.5*HEAD_HEIGHT_ODDISH+0.5, 0.0);
	glTranslatef(1.30, -0.6+NECK_HEIGHT_ODDISH+0.5*HEAD_HEIGHT_ODDISH, 0.0);

	glTranslatef(-0.5, -0.6+NECK_HEIGHT_ODDISH+0.5*HEAD_HEIGHT_ODDISH-4, 0.0);

	glPopMatrix();//add JOINT_POINT_
    glPushMatrix();//add JOINT_POINT_

    glTranslatef(-0.85*(TORSO_RADIUS_ODDISH+JOINT_POINT_RADIUS_ODDISH), 
    	0.9*TORSO_HEIGHT_ODDISH, 0.0);
    joint_point();

    glTranslatef(0.0, -0.5, 0.0);
    //glRotatef(thetaO[3], 1.0, 0.0, 0.0);
    glRotatef(thetaO[3], 1.0, 0.0, 0.0);
    glRotatef(thetaO[11], 0.0, 0.0, 1.0);
    left_upper_arm(texture3);

    glTranslatef(0.0, 0.0,UPPER_ARM_HEIGHT_ODDISH);
    joint_point();
/*
    glTranslatef(0.0, 0.1*JOINT_POINT_HEIGHT_ODDISH, 0.0);
    glRotatef(thetaO[4], 1.0, 0.0, 0.0);
    left_lower_arm();
*/
    glTranslatef(0.0, 0.0, LOWER_ARM_HEIGHT_ODDISH);
	//left_hand();

    glPopMatrix();
    glPushMatrix();

    glTranslatef(0.85*(TORSO_RADIUS_ODDISH+JOINT_POINT_RADIUS_ODDISH), 
    	0.9*TORSO_HEIGHT_ODDISH, 0.0);
    joint_point();

    glTranslatef(0.4, -0.5, 0.0);
    glRotatef(thetaO[5], 1.0, 0.0, 0.0);
    glRotatef(thetaO[12], 0.0, 0.0, 1.0);
    right_upper_arm(texture3);

    glTranslatef(0.0, 0.0, UPPER_ARM_HEIGHT_ODDISH);
    joint_point();

	//glBindTexture(GL_TEXTURE_2D, texture4);
    glTranslatef(0.0, 0.1*JOINT_POINT_HEIGHT_ODDISH, 0.0);
    glRotatef(thetaO[6], 1.0, 0.0, 0.0);
    right_lower_arm();

    glTranslatef(0.0, 0.0, LOWER_ARM_HEIGHT_ODDISH);
	//right_hand(texture3);

    glPopMatrix();
    glPushMatrix();

    glTranslatef(-(TORSO_RADIUS_ODDISH-JOINT_POINT_RADIUS_ODDISH), 
    	-0.15*JOINT_POINT_HEIGHT_ODDISH, 0.0);
	//joint_point();

    glTranslatef(0, 0.1*JOINT_POINT_HEIGHT_ODDISH, 0.0);
    glRotatef(thetaO[7], 1.0, 0.0, 0.0);
    glRotatef(thetaO[13], 0.0, 0.0, 1.0);
    //left_upper_leg();

    glTranslatef(0.0, UPPER_LEG_HEIGHT_ODDISH, 0.0);
	//joint_point();

    glTranslatef(0.0, 0.1*JOINT_POINT_HEIGHT_ODDISH, 0.0);
    glRotatef(thetaO[8], 1.0, 0.0, 0.0);
    //left_lower_leg();

    glTranslatef(0.7, 0.1*JOINT_POINT_HEIGHT_ODDISH, 0.0);
    //glTranslatef(0.0, LOWER_LEG_HEIGHT_ODDISH_ODDISH, -0.5*FOOT_HEIGHT_ODDISH);
    glRotatef(thetaO[15], 1.0, 0.0, 0.0);
    left_foot(texture3);

    glPopMatrix();
    glPushMatrix();

    glTranslatef(TORSO_RADIUS_ODDISH-JOINT_POINT_RADIUS_ODDISH, 
    	-0.15*JOINT_POINT_HEIGHT_ODDISH, 0.0);
	//joint_point();

    glTranslatef(0, 0.1*JOINT_POINT_HEIGHT_ODDISH, 0.0);
    glRotatef(thetaO[9], 1.0, 0.0, 0.0);
    glRotatef(thetaO[14], 0.0, 0.0, 1.0);
    //right_upper_leg();

    glTranslatef(0.0, UPPER_LEG_HEIGHT_ODDISH, 0.0);
	//joint_point();

    glTranslatef(0.0, 0.1*JOINT_POINT_HEIGHT_ODDISH, 0.0);
    glRotatef(thetaO[10], 1.0, 0.0, 0.0);
    //right_lower_leg();

    glTranslatef(-0.3, 0.1*JOINT_POINT_HEIGHT_ODDISH-0.2, 0.0);
	//glTranslatef(0.0, LOWER_LEG_HEIGHT_ODDISH_ODDISH, -0.5*FOOT_HEIGHT_ODDISH);
    glRotatef(thetaO[11], 1.0, 0.0, 0.0);
    right_foot(texture3);
    
    glPopMatrix();
}
Ejemplo n.º 4
0
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 
        }
}
void MarillModel::drawCyborg(GLuint texture1,GLuint texture2,GLuint texture3,GLuint texture4){
	glOrtho(-1.0, 1.0, -1.0, 1.0, -1.0, 1.0);
	glRotatef(thetaM[0], 0.0, 1.0, 0.0);
	glPushMatrix();
	glTranslatef(0,TORSO_HEIGHT_MARILL-1.0, 0.0);
	torso(texture3);
	glPopMatrix();
	glPushMatrix();

	glTranslatef(0,TORSO_HEIGHT_MARILL, 0.0);
	neck();

	glTranslatef(0.0, NECK_HEIGHT_MARILL+0.5*HEAD_HEIGHT_MARILL-0.3, 0.0);
			//glRotatef(theta[1], 1.0, 0.0, 0.0);
		    //glRotatef(theta[2], 0.0, 1.0, 0.0);

	head(texture2);

	glTranslatef(-0.70, NECK_HEIGHT_MARILL+0.5*HEAD_HEIGHT_MARILL+0.5, 0.0);
	right_ear(texture1);
	glTranslatef(1.30, -0.6+NECK_HEIGHT_MARILL+0.5*HEAD_HEIGHT_MARILL, 0.0);
	left_ear(texture1);

	glTranslatef(-0.5, -0.6+NECK_HEIGHT_MARILL+0.5*HEAD_HEIGHT_MARILL-4, 0.0);
	tail(texture4);

	glPopMatrix();//add JOINT_POINT_
    glPushMatrix();//add JOINT_POINT_

    glTranslatef(-0.85*(TORSO_RADIUS_MARILL+JOINT_POINT_RADIUS_MARILL), 
    	0.9*TORSO_HEIGHT_MARILL, 0.0);
    joint_point();

    glTranslatef(0.0, 0.0, 0.0);
    glRotatef(thetaM[3], 1.0, 0.0, 0.0);
    glRotatef(thetaM[11], 0.0, 0.0, 1.0);
    left_upper_arm(texture4);

    glTranslatef(0.0, 0.0,UPPER_ARM_HEIGHT_MARILL);
    joint_point();
/*
    glTranslatef(0.0, 0.1*JOINT_POINT_HEIGHT_MARILL, 0.0);
    glRotatef(thetaM[4], 1.0, 0.0, 0.0);
    left_lower_arm();
*/
    glTranslatef(0.0, 0.0, LOWER_ARM_HEIGHT_MARILL);
	//left_hand();

    glPopMatrix();
    glPushMatrix();

    glTranslatef(0.85*(TORSO_RADIUS_MARILL+JOINT_POINT_RADIUS_MARILL), 
    	0.9*TORSO_HEIGHT_MARILL, 0.0);
    joint_point();

    glTranslatef(0.0, 0.0, 0.0);
    glRotatef(thetaM[5], 1.0, 0.0, 0.0);
    glRotatef(thetaM[12], 0.0, 0.0, 1.0);
    right_upper_arm(texture4);

    glTranslatef(0.0, 0.0, UPPER_ARM_HEIGHT_MARILL);
    joint_point();

	//glBindTexture(GL_TEXTURE_2D, texture4);
    glTranslatef(0.0, 0.1*JOINT_POINT_HEIGHT_MARILL, 0.0);
    glRotatef(thetaM[6], 1.0, 0.0, 0.0);
    right_lower_arm();

    glTranslatef(0.0, 0.0, LOWER_ARM_HEIGHT_MARILL);
	//right_hand(texture3);

    glPopMatrix();
    glPushMatrix();

    glTranslatef(-(TORSO_RADIUS_MARILL-JOINT_POINT_RADIUS_MARILL), 
    	-0.15*JOINT_POINT_HEIGHT_MARILL, 0.0);
	//joint_point();

    glTranslatef(0, 0.1*JOINT_POINT_HEIGHT_MARILL, 0.0);
    glRotatef(thetaM[7], 1.0, 0.0, 0.0);
    glRotatef(thetaM[13], 0.0, 0.0, 1.0);
    //left_upper_leg();

    glTranslatef(0.0, UPPER_LEG_HEIGHT_MARILL, 0.0);
	//joint_point();

    glTranslatef(0.0, 0.1*JOINT_POINT_HEIGHT_MARILL, 0.0);
    glRotatef(thetaM[8], 1.0, 0.0, 0.0);
    //left_lower_leg();

    glTranslatef(0.7, 0.1*JOINT_POINT_HEIGHT_MARILL, 0.0);
    //glTranslatef(0.0, LOWER_LEG_HEIGHT_MARILL_MARILL, -0.5*FOOT_HEIGHT_MARILL);
    glRotatef(thetaM[15], 1.0, 0.0, 0.0);
    left_foot(texture4);

    glPopMatrix();
    glPushMatrix();

    glTranslatef(TORSO_RADIUS_MARILL-JOINT_POINT_RADIUS_MARILL, 
    	-0.15*JOINT_POINT_HEIGHT_MARILL, 0.0);
	//joint_point();

    glTranslatef(0, 0.1*JOINT_POINT_HEIGHT_MARILL, 0.0);
    glRotatef(thetaM[9], 1.0, 0.0, 0.0);
    glRotatef(thetaM[14], 0.0, 0.0, 1.0);
    //right_upper_leg();

    glTranslatef(0.0, UPPER_LEG_HEIGHT_MARILL, 0.0);
	//joint_point();

    glTranslatef(0.0, 0.1*JOINT_POINT_HEIGHT_MARILL, 0.0);
    glRotatef(thetaM[10], 1.0, 0.0, 0.0);
    //right_lower_leg();

    glTranslatef(-0.3, 0.1*JOINT_POINT_HEIGHT_MARILL-0.2, 0.0);
	//glTranslatef(0.0, LOWER_LEG_HEIGHT_MARILL_MARILL, -0.5*FOOT_HEIGHT_MARILL);
    glRotatef(thetaM[11], 1.0, 0.0, 0.0);
    right_foot(texture4);
    
    glPopMatrix();
}