#ifndef __MANIP_OBJ_H #define __MANIP_OBJ_H #include "Behaviors/BehaviorBase.h" #include "Events/EventRouter.h" #include "Motion/Kinematics.h" #include "Motion/HeadPointerMC.h" #include "Motion/PIDMC.h" #include "Motion/MotionPtr.h" #include "Motion/XWalkMC.h" #include "Shared/fmat.h" #include "Shared/RobotInfo.h" #include "Sound/SoundManager.h" #include "DualCoding/DualCoding.h" #include "Behaviors/Nodes/ArmNode.h" #include "Planners/RRT/RRTPlanner.h" #include "Sound/SoundManager.h" #include "Behaviors/StateMachine.h" using namespace DualCoding; #define LEFT 1 #define RIGHT 0 #define ARM_LEN 180 static int hit_dir; static float x_dis; static float y_dis; static float phi_dis; Point ball_pos; Point target_pos; //next way point extern Point next_pos; extern float next_ori; class ReachBall : public XWalkNode, public VRmixin { public: ReachBall() : XWalkNode(), VRmixin() {} float dist(float x, float y) { return sqrt(x*x + y*y); } virtual void DoStart() { fmat::Transform T = kine->linkToBase(ArmOffset); fmat::Column<3> arm_loc = fmat::SubVector<3>(&T(0,3)); Point *arm = new Point(arm_loc); Point diff = target_pos - ball_pos; float theta = atan2(diff.coordY(), diff.coordX()); // frame transformation stuff // hit left float arm_x_l = -ARM_LEN*sin(theta)+ball_pos.coordX(); float arm_y_l = +ARM_LEN*cos(theta)+ball_pos.coordY(); float robo_x_l = -sin(theta)*arm->coordX()-cos(theta)*arm->coordY() -ARM_LEN*sin(theta)+ball_pos.coordX(); float robo_y_l = cos(theta)*arm->coordX()-sin(theta)*arm->coordY() +ARM_LEN*cos(theta)+ball_pos.coordY(); float phi_l = theta - M_PI / 2; printf("phi %f\n", phi_l); // hit right float arm_x_r = ARM_LEN*sin(theta)+ball_pos.coordX(); float arm_y_r = -ARM_LEN*cos(theta)+ball_pos.coordY(); float robo_x_r = sin(theta)*arm->coordX()+cos(theta)*arm->coordY() +ARM_LEN*sin(theta)+ball_pos.coordX(); float robo_y_r = -cos(theta)*arm->coordX()+sin(theta)*arm->coordY() -ARM_LEN*cos(theta)+ball_pos.coordY(); float phi_r = theta + M_PI / 2; printf("phi %f\n", phi_r); float dist_l = dist(robo_x_l, robo_y_l); float dist_r = dist(robo_x_r, robo_y_r); if (dist_l < dist_r) { x_dis = robo_x_l; y_dis = robo_y_l; phi_dis = phi_l; hit_dir = LEFT; getMC()->setTargetDisplacement(robo_x_l, robo_y_l, phi_l); printf("hit left\n"); //for debugging purposes Point *temp = new Point(); temp->coords[0] = robo_x_l; temp->coords[1] = robo_y_l; Point *temp1 = new Point(); temp1->coords[0] = arm_x_l; temp1->coords[1] = arm_y_l; NEW_SHAPE(robot_Pos, PointData, new PointData(localShS, *temp)); NEW_SHAPE(arm_Pos, PointData, new PointData(localShS, *temp1)); NEW_SHAPE(ball_Pos, PointData, new PointData(localShS, ball_pos)); NEW_SHAPE(target_Pos, PointData, new PointData(localShS, target_pos)); } else { x_dis = robo_x_r; y_dis = robo_y_r; phi_dis = phi_r; hit_dir = RIGHT; getMC()->setTargetDisplacement(robo_x_r, robo_y_r, phi_r); printf("hit right\n"); //for debugging purposes Point *temp = new Point(); temp->coords[0] = robo_x_r; temp->coords[1] = robo_y_r; Point *temp1 = new Point(); temp1->coords[0] = arm_x_r; temp1->coords[1] = arm_y_r; NEW_SHAPE(robot_Pos, PointData, new PointData(localShS, *temp)); NEW_SHAPE(arm_Pos, PointData, new PointData(localShS, *temp1)); NEW_SHAPE(ball_Pos, PointData, new PointData(localShS, ball_pos)); NEW_SHAPE(target_Pos, PointData, new PointData(localShS, target_pos)); } } }; class LookDown : public HeadPointerNode { public: LookDown() : HeadPointerNode("LookDown") {} virtual void DoStart() { getMC()->setJoints(0, 0.167, -1); } }; class FindObstacles : public MapBuilderNode { public: FindObstacles() : MapBuilderNode("FindObstacles",MapBuilderRequest::localMap) {} virtual void DoStart() { mapreq.addObjectColor(ellipseDataType, "green"); mapreq.addObjectColor(ellipseDataType, "blue"); } }; class AdjustArm : public DynamicMotionSequenceNode, public VRmixin { public: AdjustArm() : DynamicMotionSequenceNode("ExecutePath"), VRmixin() {} virtual void DoStart() { vector obstacles; Point ball; NEW_SHAPEVEC(ellipses, EllipseData, select_type(localShS)) SHAPEVEC_ITERATE(ellipses, EllipseData, e) CircularObstacle * ob = new CircularObstacle(); ob->centerPoint = fmat::pack(e->getCentroid().coordX(), e->getCentroid().coordY()); ob->radius = max(e->getSemimajor(), e->getSemiminor()) / 1.5; cout << "Obstacle at " << ob->centerPoint[0] << "," << ob->centerPoint[1] << " with radius " << ob->radius << endl; obstacles.push_back(ob); ////////////// ball = e->getCentroid(); ///////////// END_ITERATE; unsigned int const numJoints = 3; RRTStateVector stateVec(6000, numJoints); const KinematicJoint* goodroot = kine->getKinematicJoint(ArmShoulderOffset); RRTPlanner rrtp(stateVec, goodroot, numJoints, NULL, 1000, 0.0001f, M_PI/60.0f, M_PI/120.0f); /* Generate the instance of the planner stateVec - RRTStateVector* for where to store the states used in planning goodroot - The start of the kinematics chain you want to use numJoints (optional) - How many joints down the kinematic chain you want to go 1000 (optional) - The maximum number of iterations you want the planner to go through 0.4f (optional) - The allowed error between two states to call them "equal". Make lower for better results, but may not converge M_PI/30.0f (optional) - The largest change each joint can change by between states M_PI/120.0f (optional) - The largest change each joint can change by during an interpolation check (finer grain so it won't miss any obstacles) */ RRTStateDef startSt(numJoints), endSt(numJoints); for(unsigned int i = 0; i < numJoints; i++) startSt[i] = state->outputs[ArmShoulderOffset + i]; fmat::Transform T = kine->linkToBase(ArmOffset); fmat::Column<3> arm_loc = fmat::SubVector<3>(&T(0,3)); Point *arm = new Point(arm_loc); Point arm2ob = ball - *arm; float angle = atan2(arm2ob.coordY(), arm2ob.coordX()); //////////////////////////////////////////////// // left float t_angle; if (hit_dir == LEFT) t_angle = angle - 0.4; // right else t_angle = angle + 0.4; endSt[0] = t_angle; endSt[1] = t_angle; endSt[2] = t_angle; //////////////////////////////////////////////// const KinematicJoint* gripper = kine->getKinematicJoint(GripperFrameOffset); fmat::Column<3> where = gripper->getWorldPosition(); NEW_SHAPE(tgt, PointData, new PointData(localShS, DualCoding::Point(where[0], where[1], where[2]))); RRTPath path; if ( !rrtp.Plan(path, startSt, endSt, &obstacles) ) { return; } getMC()->clear(); generatePostures(path); getMC()->play(); } void generatePostures(const RRTPath& path) { cout << "Making postures..." << endl; getMC()->setTime(3000); for(unsigned int i = 0; i < path.size(); i++) { cout << "Step " << i << " "; for(unsigned int j = 0; j < path[i].size(); j++) { cout << " " << j << ": " << path[i][j]; getMC()->setOutputCmd(ArmOffset+j, path[i][j]); } cout << endl; getMC()->advanceTime(1000); } } }; class HitBall: public ArmNode { public: HitBall(): ArmNode("PushObjects") {} void DoStart() { getMC()->setMaxSpeed(0, 3); getMC()->setMaxSpeed(1, 3); getMC()->setMaxSpeed(2, 3); getMC()->setJointValue(1, 0); getMC()->setJointValue(2, 0); if (hit_dir == LEFT) { getMC()->setJointValue(0, 0.8); } else { getMC()->setJointValue(0, -0.8); } } }; class ResetPose : public XWalkNode { public: ResetPose() : XWalkNode() {} virtual void DoStart() { printf("reset: %f %f %f\n", -x_dis, -y_dis, -phi_dis); getMC()->setTargetDisplacement(-x_dis, -y_dis, -phi_dis); } }; class Move2WayPoint: public XWalkNode { public: Move2WayPoint(): XWalkNode() {} virtual void DoStart() { float next_angle = atan2(next_pos.coordY(), next_pos.coordX()); printf("=============> turn %f %f %f \n", next_pos.coordX(), next_pos.coordY(), next_angle); getMC()->setTargetDisplacement(next_pos.coordX(), next_pos.coordY(), next_angle); } }; class RotAtWayPoint: public XWalkNode { public: RotAtWayPoint(): XWalkNode() {} virtual void DoStart() { getMC()->setTargetDisplacement(0, 0, next_ori); } }; #endif