#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 200 int hit_dir; Point ball_pos; Point target_pos; class Manipulation : public VisualRoutinesStateNode { public: 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) { getMC()->setTargetDisplacement(robo_x_l, robo_y_l, phi_l); printf("hit left\n"); hit_dir = LEFT; //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 { getMC()->setTargetDisplacement(robo_x_r, robo_y_r, phi_r); printf("hit right\n"); hit_dir = RIGHT; //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, "orange"); } }; class ExecutePath : public DynamicMotionSequenceNode { public: ExecutePath() : DynamicMotionSequenceNode("ExecutePath") {} 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()); 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.5; // right else t_angle = angle + 0.5; 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) ) { postStateSignal(0); 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 VRmixin { public: HitBall(): ArmNode("PushObjects"), VRmixin() {} 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); } } }; Manipulation() : VisualRoutinesStateNode("manip test") {} virtual void setup() { //clean up HERE ////////////////////////////////// ball_pos.coords[0] = 400; ball_pos.coords[1] = 50; ball_pos.coords[2] = 0; target_pos.coords[0] = 500; target_pos.coords[1] = -50; ////////////////////////////////// #statemachine startnode: LookDown() =C=> loop: StateNode =B(GreenButOffset)=> reach: ReachBall() =C=> dummy: StateNode =B(GreenButOffset)=> FindObstacles() =MAP=> exec: ExecutePath() =C=> StateNode =T(1500)=> HitBall() =C=> loop exec =S=> SpeechNode("No path found.") =T(2500)=> loop #endstatemachine } }; #endif