moveRobot.asv

% Define Gripper Position
% Based on the information provided by the Vision :V

gripperX = 0.207;
gripperY = 0.3025;
gripperZ = 0.68;
gripperTranslation = [gripperX gripperY gripperZ];
gripperRotationX = 0; % radians
gripperRotationY = pi; % radians
gripperRotationZ = pi/2; % radians
desiredGripperPose = trvec2tform(gripperTranslation)*axang2tform([1 0 0 gripperRotationX])*axang2tform([0 1 0 gripperRotationY])*axang2tform([0 0 1 gripperRotationZ]);
% Compute Trajectory

jointMsg = receive(jointSub,2);
currentRobotJConfig =  jointMsg.Position(2:8);
[q,qd,qdd,trajTimes] = RoboCupManipulation_computeTrajectory(currentRobotJConfig, desiredGripperPose, robot, 'gripper', 4);

% Package and send the joint trajectory 

trajGoal = RoboCupManipulation_packageJointTrajectory(trajGoal,q,qd,qdd,trajTimes);
waitForServer(trajAct);
sendGoal(trajAct,trajGoal);