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;
}
