void KickEngineData::initData(const FrameInfo& frame, const KickRequest& kr, std::vector<KickEngineParameters>& params, const JointAngles& ja, const TorsoMatrix& torsoMatrix, JointRequest& jointRequest, const RobotDimensions& rd, const MassCalibration& mc, const DamageConfigurationBody& theDamageConfigurationBody) { VERIFY(getMotionIDByName(kr, params)); phase = 0.f; phaseNumber = 0; timeStamp = frame.time; currentParameters = params[motionID]; toLeftSupport = currentParameters.standLeft; ref = Vector3f::Zero(); actualDiff = ref; calculateOrigins(kr, ja, torsoMatrix, rd); currentParameters.initFirstPhase(origins, Vector2f(ja.angles[Joints::headPitch], (kr.mirror) ? -ja.angles[Joints::headYaw] : ja.angles[Joints::headYaw])); calcPositions(); float angleY = (toLeftSupport) ? -robotModel.limbs[Limbs::footLeft].rotation.inverse().getYAngle() : -robotModel.limbs[Limbs::footRight].rotation.inverse().getYAngle(); float angleX = (toLeftSupport) ? -robotModel.limbs[Limbs::footLeft].rotation.inverse().getXAngle() : -robotModel.limbs[Limbs::footRight].rotation.inverse().getXAngle(); if(kr.mirror) angleX *= -1.f; bodyAngle = Vector2f(angleX, angleY); calcJoints(jointRequest, rd, theDamageConfigurationBody); comRobotModel.setJointData(jointRequest, rd, mc); const Pose3f& torso = toLeftSupport ? comRobotModel.limbs[Limbs::footLeft] : comRobotModel.limbs[Limbs::footRight]; const Vector3f com = torso.rotation.inverse() * comRobotModel.centerOfMass; //this calculates inverse of pid com control -> getting com to max pos at start -> beeing rotated as before engine float foot = toLeftSupport ? origins[Phase::leftFootTra].z() : origins[Phase::rightFootTra].z(); float height = comRobotModel.centerOfMass.z() - foot; balanceSum.x() = std::tan(angleY - Constants::pi) * height; balanceSum.x() /= currentParameters.kiy; balanceSum.y() = std::tan(angleX - Constants::pi) * height; balanceSum.y() /= -currentParameters.kix; currentParameters.initFirstPhaseLoop(origins, Vector2f(com.x(), com.y()), Vector2f(ja.angles[Joints::headPitch], (kr.mirror) ? -ja.angles[Joints::headYaw] : ja.angles[Joints::headYaw])); if(!wasActive) { // origin = Vector2f::Zero(); gyro = Vector2f::Zero(); lastGyroLeft = Vector2f::Zero(); lastGyroRight = Vector2f::Zero(); gyroErrorLeft = Vector2f::Zero(); gyroErrorRight = Vector2f::Zero(); bodyError = Vector2f::Zero(); lastBody = Vector2f::Zero(); lastCom = Vector3f::Zero(); for(int i = 0; i < Joints::numOfJoints; i++) { lastBalancedJointRequest.angles[i] = ja.angles[i]; } } for(unsigned int i = 0; i < kr.dynPoints.size(); i++) if(kr.dynPoints[i].phaseNumber == phaseNumber) addDynPoint(kr.dynPoints[i], torsoMatrix); lElbowFront = origins[Phase::leftHandRot].x() > pi_4; rElbowFront = origins[Phase::rightHandRot].x() < -pi_4; if(kr.armsBackFix) //quick hack to not break arms while they are on the back { if(lElbowFront) addDynPoint(DynPoint(Phase::leftHandRot, 0, Vector3f(pi_2, -pi_4, 0)), torsoMatrix); if(rElbowFront) addDynPoint(DynPoint(Phase::rightHandRot, 0, Vector3f(-pi_2, pi_4, 0)), torsoMatrix); } }
int main(int argc, char **argv) { pending_bricks = std::vector<bpMsgs::robot_pick>(); /* ros messages */ /* parameters */ std::string brick_subscriber_topic; std::string brick_publisher_topic; std::string emergency_stop_subscriber_topic; int loop_rate_param; /* initialize ros usage */ ros::init(argc, argv, "robot_motion_controller"); ros::Publisher brick_publisher; ros::Subscriber brick_subscriber; ros::Subscriber emergency_stop_subscriber; bpPLCController::plc_command plcCommand; plcCommand.request.command_number = bpPLCController::plc_command::Request::GET_GRIPPER_SENSOR_STATUS; /* private nodehandlers */ ros::NodeHandle nh; ros::NodeHandle n("~"); /* read parameters from ros parameter server if available otherwise use default values */ n.param<std::string> ("brick_subscriber_topic", brick_subscriber_topic, "/bpRobotMotionController/brick_command_topic"); n.param<std::string> ("brick_publisher_topic", brick_publisher_topic, "/bpRobotMotionController/brick_response_topic"); n.param<std::string> ("emergency_stop_subscriber_topic", emergency_stop_subscriber_topic, "/emergency_stop"); n.param<int> ("loop_rate", loop_rate_param, 1); brick_publisher = nh.advertise<bpMsgs::robot_pick> (brick_publisher_topic.c_str(), 10); brick_subscriber = nh.subscribe<bpMsgs::robot_pick> (brick_subscriber_topic.c_str(), 20, brickHandler); emergency_stop_subscriber = nh.subscribe<std_msgs::Bool> (emergency_stop_subscriber_topic.c_str(),20,emergencyStopHandler); setKnownConfs(); robot_client = n.serviceClient<bpDrivers::command>("/bpDrivers/rx60_controller/rx60_command"); ros::ServiceClient plc_client = n.serviceClient<bpPLCController::plc_command>("/plc_controller/plc_command"); std::cout << "RobotMotionPlanner node started! - waiting for RX60 controller service" << std::endl; robot_client.waitForExistence(); std::cout << "RX60 controller service found! - Going to idle position" << std::endl; robot_state = idle; // Go to Idle position robot_client.call(cmdIdle,cmdRes); robot_client.call(cmdOpenGripper, cmdRes); bpMsgs::robot_pick brickToPick; bpMsgs::robot_pick returnMessage; ros::Rate loop_rate(loop_rate_param); while (ros::ok()) { ros::spinOnce(); switch (robot_state) { case idle: if (pending_bricks.size() > 0) { brickToPick = pending_bricks[0]; pending_bricks.erase(pending_bricks.begin()); if (!calcPositions(brickToPick)) { // Cant reach brick robot_state = failed; } else { robot_client.call(cmdWaitPosition,cmdRes); ros::Duration(0.05).sleep(); robot_state = wait_for_wait_position; } } else { robot_client.call(cmdIdle,cmdRes); } break; case wait_for_wait_position: cmdReq.command_number = bpDrivers::commandRequest::IS_SETTLED; robot_client.call(cmdReq,cmdRes); if (cmdRes.is_settled) { robot_state = go_down_to_pick_brick; } break; case go_down_to_pick_brick: if (brickToPick.header.stamp.toSec() < (ros::Time::now().toSec()) ) { robot_state = failed; } else { ros::Duration( (brickToPick.header.stamp.toSec() - ros::Time::now().toSec()) - 0.20).sleep(); robot_client.call(cmdGripPosition,cmdRes); robot_state = wait_for_brick;//wait_for_grip_position; ros::Duration(0.05).sleep(); } break; case wait_for_grip_position: cmdReq.command_number = bpDrivers::commandRequest::IS_SETTLED; robot_client.call(cmdReq,cmdRes); if (cmdRes.is_settled) { robot_state = wait_for_brick; } break; case wait_for_brick: if (brickToPick.header.stamp.toSec() < (ros::Time::now().toSec() - 0.0) ) { robot_client.call(cmdCloseGripper,cmdRes); ros::Duration(0.3).sleep(); plc_client.call(plcCommand); ROS_INFO("Gripper1: %d", plcCommand.response.result); if (!plcCommand.response.result) { robot_state = go_to_delivery_position; } else { robot_state = failed; robot_client.call(cmdWaitPosition,cmdRes); robot_client.call(cmdOpenGripper,cmdRes); } } break; case go_to_delivery_position: robot_client.call(cmdWaitPosition,cmdRes); robot_client.call(cmdOrder[brickToPick.order-1],cmdRes); robot_client.call(cmdOrderDropoff[brickToPick.order-1],cmdRes); robot_state = wait_for_delivery_position; ros::Duration(0.1).sleep(); break; case wait_for_delivery_position: cmdReq.command_number = bpDrivers::commandRequest::IS_SETTLED; robot_client.call(cmdReq,cmdRes); if (cmdRes.is_settled) { plc_client.call(plcCommand); ROS_INFO("Gripper2: %d", plcCommand.response.result); if (!plcCommand.response.result) { robot_client.call(cmdOpenGripper,cmdRes); ros::Duration(0.15).sleep(); robot_state = go_to_safe_position; } else { robot_client.call(cmdOpenGripper,cmdRes); robot_client.call(cmdOrder[brickToPick.order-1],cmdRes); ros::Duration(0.1).sleep(); robot_state = failed; } } break; case go_to_safe_position: robot_client.call(cmdOrder[brickToPick.order-1],cmdRes); ros::Duration(0.1).sleep(); robot_state = wait_for_safe_position; break; case wait_for_safe_position: cmdReq.command_number = bpDrivers::commandRequest::IS_SETTLED; robot_client.call(cmdReq,cmdRes); if (cmdRes.is_settled) { robot_state = succeded; } break; case succeded: returnMessage = brickToPick; returnMessage.succes = true; brick_publisher.publish(returnMessage); robot_state = idle; break; case failed: ROS_INFO("ERROR cant reach brick"); returnMessage = brickToPick; returnMessage.succes = false; brick_publisher.publish(returnMessage); robot_state = idle; break; case emergency_stop: break; default: break; } // check if robot is settled //cmdReq.command_number = bpDrivers::commandRequest::IS_SETTLED; //client.call(cmdReq,cmdRes); //if (cmdRes.is_settled); loop_rate.sleep(); } return 0; }