Raw File
RaptorController.cpp
#include "RaptorController.h"
#include <iostream>
#include <ctime>
#include <json/json.h>

#include "sim/SimCharacter.h"
#include "sim/RBDUtil.h"
#include "util/FileUtil.h"

const cRaptorController::eStance gDefaultStance = cRaptorController::eStanceRight;
const cRaptorController::tStateDef gStateDefs[cRaptorController::eStateMax] =
{
	{
		"Contact",
		true,									// mTransTime;
		false,									// mTransContact;
		cRaptorController::eStateDown			// mNext;
	},
	{
		"Down",
		true,									// mTransTime;
		false,									// mTransFootContact;
		cRaptorController::eStatePassing		// mNext;
	},
	{
		"Passing",
		true,									// mTransTime;
		false,									// mTransContact;
		cRaptorController::eStateUp				// mNext;
	},
	{
		"Up",
		false,									// mTransTime;
		true,									// mTransContact;
		cRaptorController::eStateInvalid		// mNext;
	},
};

const std::string gMiscParamsNames[cRaptorController::eMiscParamMax] = {
	"TransTime",
	"Cv",
	"Cd",
	"ForceX",
	"ForceY"
};

const std::string gStateParamNames[cRaptorController::eStateParamMax] = {
	"RootPitch",
	"SpineCurve",
	"StanceHip",
	"StanceKnee",
	"StanceAnkle",
	"SwingHip",
	"SwingKnee",
	"SwingAnkle"
};

const cSimRaptor::eJoint gEndEffectors[] = {
	cSimRaptor::eJointRightToe,
	cSimRaptor::eJointLeftToe,
};
const int gNumEndEffectors = sizeof(gEndEffectors) / sizeof(gEndEffectors[0]);

const cSimRaptor::eJoint gSpineJoints[] = {
	cSimRaptor::eJointSpine0,
	cSimRaptor::eJointSpine1,
	cSimRaptor::eJointSpine2,
	cSimRaptor::eJointSpine3
};
const int gNumSpineJoints = sizeof(gSpineJoints) / sizeof(gSpineJoints[0]);

const std::string gMiscParamsKey = "MiscParams";
const std::string gStateParamsKey = "StateParams";
const std::string gControllersKey = "Controllers";
const std::string gFilesKey = "Files";
const std::string gActionsKey = "Actions";

const bool gOptParamsMasks[] =
{
	false,	//TransTime
	true,	//Cv
	true,	//Cd
	false,	//ForceX
	false,	//ForceY

	//Contact
	true,	//RootPitch
	false,	//SpineCurve
	true,	//StanceHip
	true,	//StanceKnee
	true,	//StanceAnkle
	true,	//SwingHip
	true,	//SwingKnee
	true,	//SwingAnkle

	//Down
	true,	//RootPitch
	false,	//SpineCurve
	true,	//StanceHip
	true,	//StanceKnee
	true,	//StanceAnkle
	true,	//SwingHip
	true,	//SwingKnee
	true,	//SwingAnkle

	//Passing
	false,	//RootPitch
	false,	//SpineCurve
	true,	//StanceHip
	true,	//StanceKnee
	true,	//StanceAnkle
	true,	//SwingHip
	true,	//SwingKnee
	true,	//SwingAnkle

	//Up
	false,	//RootPitch
	false,	//SpineCurve
	true,	//StanceHip
	true,	//StanceKnee
	true,	//StanceAnkle
	true,	//SwingHip
	true,	//SwingKnee
	true	//SwingAnkle
};
const int gNumOptParamMasks = sizeof(gOptParamsMasks) / sizeof(gOptParamsMasks[0]);

cRaptorController::cRaptorController() : cTerrainRLCharController()
{
	mDefaultAction = gInvalidIdx;
	mState = eStateContact;
	mEnableGravityCompensation = true;
	mEnableVirtualForces = true;
}

cRaptorController::~cRaptorController()
{
}

void cRaptorController::Init(cSimCharacter* character, const tVector& gravity, const std::string& param_file)
{
	// param_file should contain parameters for the pd controllers
	cTerrainRLCharController::Init(character);

	mGravity = gravity;
	mCtrlParams.clear();
	mPrevCOM = character->CalcCOM();

	Eigen::MatrixXd pd_params;
	bool succ = cPDController::LoadParams(param_file, pd_params);

	if (succ)
	{
		mRBDModel = std::shared_ptr<cRBDModel>(new cRBDModel());
		mRBDModel->Init(mChar->GetJointMat(), mChar->GetBodyDefs(), mGravity);

		mImpPDCtrl.Init(mChar, mRBDModel, pd_params, mGravity);
		
		succ = LoadControllers(param_file);
		TransitionState(eStateContact);
	}

	if (succ)
	{
		mValid = true;
		SetStance(gDefaultStance);
	}
	else
	{
		printf("Failed to initialize dog controller\n");
		mValid = false;
	}
}

void cRaptorController::Reset()
{
	cTerrainRLCharController::Reset();
	ClearCommands();
	mImpPDCtrl.Reset();
	SetStance(gDefaultStance);
	mPrevCOM = mChar->CalcCOM();
}

void cRaptorController::Clear()
{
	cTerrainRLCharController::Clear();
	mImpPDCtrl.Clear();
	mCtrlParams.clear();
	ClearCommands();

	mRBDModel.reset();
	mDefaultAction = gInvalidIdx;
}

void cRaptorController::Update(double time_step)
{
	cTerrainRLCharController::Update(time_step);

	int num_dof = mChar->GetNumDof();
	Eigen::VectorXd tau = Eigen::VectorXd::Zero(num_dof);

	if (mMode == eModeActive)
	{
		mCurrCycleTime += time_step;
		UpdateStumbleCounter(time_step);
		
		UpdateRBDModel();
		UpdateState(time_step);
		UpdateStanceHip();

		// order matters!
		ApplySwingFeedback(tau);
		UpdatePDCtrls(time_step, tau);

		if (mEnableGravityCompensation)
		{
			ApplyGravityCompensation(tau);
		}
		
		ApplyStanceFeedback(tau);

		if (mEnableVirtualForces)
		{
			ApplyVirtualForces(tau);
		}
	}
	else if (mMode == eModePassive)
	{
		UpdatePDCtrls(time_step, tau);
	}

	mChar->ApplyControlForces(tau);
}

void cRaptorController::SeCtrlStateParams(eState state, const tStateParams& params)
{
	mCurrAction.mParams.segment(eMiscParamMax + state * eStateParamMax, eStateParamMax) = params;
}

void cRaptorController::TransitionState(int state)
{
	cTerrainRLCharController::TransitionState(state);
}

void cRaptorController::TransitionState(int state, double phase)
{
	assert(state >= 0 && state < eStateMax);
	cTerrainRLCharController::TransitionState(state, phase);

	tStateParams params = GetCurrParams();
	SetStateParams(params);
}

void cRaptorController::SetTransTime(double time)
{
	// set duration of each state in the walk
	mCurrAction.mParams[eMiscParamTransTime] = time;
}

int cRaptorController::GetNumStates() const
{
	return eStateMax;
}

void cRaptorController::SetMode(eMode mode)
{
	cTerrainRLCharController::SetMode(mode);

	if (mMode == eModePassive)
	{
		SetupPassiveMode();
	}
}

void cRaptorController::CommandAction(int action_id)
{
	int num_actions = GetNumActions();
	if (action_id < num_actions)
	{
		mCommands.push(action_id);
	}
	else
	{
		assert(false); // invalid action
	}
}

void cRaptorController::CommandRandAction()
{
	int num_actions = GetNumActions();
	int a = cMathUtil::RandInt(0, num_actions);
	CommandAction(a);
}

int cRaptorController::GetDefaultAction() const
{
	return mDefaultAction;
}

void cRaptorController::SetDefaultAction(int action_id)
{
	if (action_id >= 0 && action_id < GetNumActions())
	{
		mDefaultAction = action_id;
	}
}

int cRaptorController::GetNumActions() const
{
	return static_cast<int>(mActions.size());
}

void cRaptorController::BuildCtrlOptParams(int ctrl_params_idx, Eigen::VectorXd& out_params) const
{
	GetOptParams(mCtrlParams[ctrl_params_idx], out_params);
}

void cRaptorController::SetCtrlParams(int ctrl_params_id, const Eigen::VectorXd& params)
{
	assert(params.size() == GetNumParams());
	Eigen::VectorXd& ctrl_params = mCtrlParams[ctrl_params_id];
	ctrl_params = params;
	PostProcessParams(ctrl_params);

	const tBlendAction& curr_action = mActions[mCurrAction.mID];
	if (curr_action.mParamIdx0 == ctrl_params_id
		|| curr_action.mParamIdx1 == ctrl_params_id)
	{
		ApplyAction(mCurrAction.mID);
	}
}

void cRaptorController::SetCtrlOptParams(int ctrl_params_id, const Eigen::VectorXd& opt_params)
{
	assert(opt_params.size() == GetNumOptParams());
	Eigen::VectorXd& ctrl_params = mCtrlParams[ctrl_params_id];
	SetOptParams(opt_params, ctrl_params);

	const tBlendAction& curr_action = mActions[mCurrAction.mID];
	if (curr_action.mParamIdx0 == ctrl_params_id
		|| curr_action.mParamIdx1 == ctrl_params_id)
	{
		ApplyAction(mCurrAction.mID);
	}
}

void cRaptorController::BuildActionOptParams(int action_id, Eigen::VectorXd& out_params) const
{
	assert(action_id >= 0 && action_id < GetNumActions());
	const tBlendAction& action = mActions[action_id];
	Eigen::VectorXd params;
	BlendCtrlParams(action, params);
	GetOptParams(params, out_params);
}

int cRaptorController::GetNumParams() const
{
	int num_params = eMiscParamMax + eStateMax * eStateParamMax;
	return num_params;
}

int cRaptorController::GetNumOptParams() const
{
	int num_params = GetNumParams();
	assert(gNumOptParamMasks == num_params);
	
	int num_opt_params = 0;
	for (int i = 0; i < num_params; ++i)
	{
		if (IsOptParam(i))
		{
			++num_opt_params;
		}
	}

	return num_opt_params;
}

void cRaptorController::FetchOptParamScale(Eigen::VectorXd& out_scale) const
{
	int num_opt_params = GetNumOptParams();
	int num_params = GetNumParams();

	const double angle_scale = M_PI / 10;
	const double cv_scale = 0.02;
	const double cd_scale = 0.2;
	const double force_scale = 100;
	const double spine_curve_scale = 0.02;

	Eigen::VectorXd param_buffer = Eigen::VectorXd::Ones(num_params);

	int idx = 0;
	for (int i = 0; i < eMiscParamMax; ++i)
	{
		if (i == eMiscParamCv)
		{
			param_buffer[idx] = cv_scale;
		}
		else if (i == eMiscParamCd)
		{
			param_buffer[idx] = cd_scale;
		}
		else if (i == eMiscParamForceX || i == eMiscParamForceY)
		{
			param_buffer[idx] = force_scale;
		}

		++idx;
	}

	for (int i = 0; i < eStateMax; ++i)
	{
		for (int j = 0; j < eStateParamMax; ++j)
		{
			if (j == eStateParamSpineCurve)
			{
				param_buffer[idx] = spine_curve_scale;
			}
			else
			{
				param_buffer[idx] = angle_scale;
			}
			++idx;
		}
	}

	assert(idx == num_params);
	GetOptParams(param_buffer, out_scale);
}

void cRaptorController::OutputOptParams(const std::string& file, const Eigen::VectorXd& params) const
{
	cController::OutputOptParams(file, params);
}

void cRaptorController::OutputOptParams(FILE* f, const Eigen::VectorXd& params) const
{
	std::string opt_param_json = BuildOptParamsJson(params);
	fprintf(f, "%s\n", opt_param_json.c_str());
}

void cRaptorController::ReadParams(std::ifstream& f_stream)
{
	Json::Reader reader;
	Json::Value root;
	bool succ = reader.parse(f_stream, root);
	if (succ)
	{
		double trans_time = 0;
		Eigen::VectorXd param_vec = Eigen::VectorXd::Zero(GetNumParams());
		int idx = 0;
		// grab the transition time first
		if (!root[gMiscParamsKey].isNull())
		{
			Json::Value misc_params = root[gMiscParamsKey];
			for (int i = 0; i < eMiscParamMax; ++i)
			{
				param_vec[idx++] = misc_params[gMiscParamsNames[i]].asDouble();
			}
		}
		else
		{
			printf("Failed to find %s\n", gMiscParamsKey.c_str());
		}

		if (!root[gStateParamsKey].isNull())
		{
			Json::Value state_params = root[gStateParamsKey];

			for (int i = 0; i < eStateMax; ++i)
			{
				Json::Value curr_params = state_params[gStateDefs[i].mName];
				for (int j = 0; j < eStateParamMax; ++j)
				{
					param_vec[idx++] = curr_params[gStateParamNames[j]].asDouble();
				}
			}
		}
		else
		{
			printf("Failed to find %s\n", gStateParamsKey.c_str());
		}

		if (!HasCtrlParams())
		{
			SetParams(param_vec);

			// refresh controls params
			tStateParams curr_params = GetCurrParams();
			SetStateParams(curr_params);
		}
		PostProcessParams(param_vec);
		mCtrlParams.push_back(param_vec);
	}
}

void cRaptorController::ReadParams(const std::string& file)
{
	cTerrainRLCharController::ReadParams(file);
}

void cRaptorController::SetParams(const Eigen::VectorXd& params)
{
	assert(params.size() == GetNumParams());
	mCurrAction.mParams = params;
	PostProcessParams(mCurrAction.mParams);
}

void cRaptorController::BuildOptParams(Eigen::VectorXd& out_params) const
{
	GetOptParams(mCurrAction.mParams, out_params);
}

void cRaptorController::SetOptParams(const Eigen::VectorXd& opt_params)
{
	SetOptParams(opt_params, mCurrAction.mParams);
}

void cRaptorController::SetOptParams(const Eigen::VectorXd& opt_params, Eigen::VectorXd& out_params) const
{
	assert(opt_params.size() == GetNumOptParams());
	assert(gNumOptParamMasks == GetNumParams());

	int num_params = GetNumParams();
	int opt_idx = 0;
	for (int i = 0; i < num_params; ++i)
	{
		if (IsOptParam(i))
		{
			out_params[i] = opt_params[opt_idx];
			++opt_idx;
		}
	}

	PostProcessParams(out_params);
}

void cRaptorController::BuildFromMotion(int ctrl_params_idx, const cMotion& motion)
{
	assert(motion.IsValid());
	double dur = motion.GetDuration();
	dur -= 0.00001; // hack to prevent flipping stance at the beginning of a new cycle
	int num_states = GetNumStates();
	double trans_time = dur / num_states;
	SetTransTime(trans_time);

	Eigen::VectorXd params = mCurrAction.mParams;
	for (int s = 0; s < num_states; ++s)
	{
		double curr_time = (s + 1) * trans_time;
		Eigen::VectorXd curr_pose = motion.CalcFrame(curr_time);

		tStateParams state_params = tStateParams::Zero();
		BuildStateParamsFromPose(curr_pose, state_params);
		params.segment(eMiscParamMax + s * eStateParamMax, eStateParamMax) = state_params;
	}

	SetCtrlParams(ctrl_params_idx, params);
}

double cRaptorController::CalcReward() const
{
	tVector target_vel = GetTargetVel();
	
	const double vel_reward_w = 0.8;
	double vel_reward = 0;
	const double stumble_reward_w = 0.2;
	double stumble_reward = 0;

	bool fallen = mChar->HasFallen();
	if (!fallen)
	{
		double cycle_time = GetPrevCycleTime();
		double avg_vel = mPrevDistTraveled[0] / cycle_time;
		double vel_err = target_vel[0] - avg_vel;
		double vel_gamma = 0.5;
		vel_reward = std::exp(-vel_gamma * vel_err * vel_err);

		double stumble_count = mPrevStumbleCount;
		double avg_stumble = stumble_count /= cycle_time;
		double stumble_gamma = 10;
		stumble_reward = 1.0 / (1 + stumble_gamma * avg_stumble);

		if (avg_vel < 0)
		{
			vel_reward = 0;
			stumble_reward = 0;
		}
	}
	
	double reward = 0;
	reward += vel_reward_w * vel_reward
			+ stumble_reward_w * stumble_reward;

	return reward;
}

tVector cRaptorController::GetTargetVel() const
{
	return tVector(4, 0, 0, 0);
	//return tVector(1.75, 0, 0, 0);
}

cRaptorController::eStance cRaptorController::GetStance() const
{
	return mStance;
}

double cRaptorController::GetPrevCycleTime() const
{
	return mPrevCycleTime;
}

const tVector& cRaptorController::GetPrevDistTraveled() const
{
	return mPrevDistTraveled;
}

void cRaptorController::BuildNormPose(Eigen::VectorXd& pose) const
{
	if (mChar != nullptr)
	{
		mChar->BuildPose(pose);
		eStance stance = GetStance();
		if (stance != gDefaultStance)
		{
			FlipPoseStance(pose);
		}
	}
}

bool cRaptorController::LoadControllers(const std::string& file)
{
	std::ifstream f_stream(file);
	Json::Reader reader;
	Json::Value root;
	bool succ = reader.parse(f_stream, root);
	if (succ)
	{
		if (!root[gControllersKey].isNull())
		{
			Json::Value ctrl_root = root[gControllersKey];
			succ = ParseControllers(ctrl_root);
		}
	}

	if (succ)
	{
		if (mDefaultAction != gInvalidIdx)
		{
			ApplyAction(mDefaultAction);
		}
	}
	else
	{
		printf("failed to parse controllers from %s\n", file.c_str());
		assert(false);
	}

	return succ;
}

void cRaptorController::ResetParams()
{
	cTerrainRLCharController::ResetParams();
	mState = eStateContact;
	mStance = gDefaultStance;
	
	mPrevCycleTime = 0;
	mPrevDistTraveled.setZero();
	mCurrCycleTime = 0;
	mPrevCOM.setZero();
	mPrevStumbleCount = 0;
	mCurrStumbleCount = 0;
}

bool cRaptorController::ParseControllers(const Json::Value& root)
{
	bool succ = true;
	
	if (!root[gFilesKey].isNull())
	{
		succ &= ParseControllerFiles(root[gFilesKey]);
	}

	if (succ && !root[gActionsKey].isNull())
	{
		succ &= ParseActions(root[gActionsKey]);

		int num_action = GetNumActions();
		if (succ && num_action > 0)
		{
			mDefaultAction = 0;
			if (!root["DefaultAction"].isNull())
			{
				mDefaultAction = root["DefaultAction"].asInt();
				assert(mDefaultAction >= 0 && mDefaultAction < num_action);
			}
		}

		if (!root["EnableGravityCompensation"].isNull())
		{
			mEnableGravityCompensation = root["EnableGravityCompensation"].asBool();
		}

		if (!root["EnableVirtualForces"].isNull())
		{
			mEnableVirtualForces = root["EnableVirtualForces"].asBool();
		}
	}

	return succ;
}

bool cRaptorController::ParseControllerFiles(const Json::Value& root)
{
	bool succ = true;
	std::vector<std::string> files;
	assert(root.isArray());

	int num_files = root.size();
	files.resize(num_files);
	for (int f = 0; f < num_files; ++f)
	{
		std::string curr_file = root.get(f, 0).asString();
		ReadParams(curr_file);
	}
	return succ;
}

bool cRaptorController::ParseActions(const Json::Value& root)
{
	bool succ = true;
	assert(root.isArray());

	int num_actions = root.size();
	for (int a = 0; a < num_actions; ++a)
	{
		const Json::Value& curr_action = root.get(a, 0);
		tBlendAction action;
		succ &= ParseAction(curr_action, action);
		if (succ)
		{
			action.mID = static_cast<int>(mActions.size());
			mActions.push_back(action);
		}
		else
		{
			succ = false;
			break;
		}
	}

	if (!succ)
	{
		printf("failed to parse actions\n");
		assert(false);
	}
	return succ;
}

bool cRaptorController::ParseAction(const Json::Value& root, tBlendAction& out_action) const
{
	if (!root["ParamIdx0"].isNull())
	{
		out_action.mParamIdx0 = root["ParamIdx0"].asInt();
	}
	else
	{
		return false;
	}
	if (!root["ParamIdx1"].isNull())
	{
		out_action.mParamIdx1 = root["ParamIdx1"].asInt();
	}
	else
	{
		return false;
	}
	if (!root["Blend"].isNull())
	{
		out_action.mBlend = root["Blend"].asDouble();
	}
	else
	{
		return false;
	}
	if (!root["Cyclic"].isNull())
	{
		out_action.mCyclic = root["Cyclic"].asBool();
	}
	else
	{
		return false;
	}
	return true;
}

bool cRaptorController::HasCtrlParams() const
{
	return mCtrlParams.size() > 0;
}

void cRaptorController::UpdateState(double time_step)
{
	const cRaptorController::tStateDef& state = GetCurrStateDef();
	bool advance_state = mFirstCycle;

	double trans_time = GetTransTime();
	mPhase += time_step / trans_time;

	if (state.mTransTime)
	{
		if (mPhase >= 1)
		{
			advance_state = true;
		}
	}

	if (state.mTransContact)
	{
		int swing_toe = GetSwingToe();
		bool contact = CheckContact(swing_toe);
		if (contact)
		{
			advance_state = true;
		}
	}

	if (advance_state)
	{
		eState next_state = (mFirstCycle) ? eStateContact : state.mNext;
		bool end_step = (next_state == eStateInvalid) || mFirstCycle;
		
		if (end_step)
		{
			if (!mFirstCycle)
			{
				FlipStance();
			}
			UpdateAction();
			mFirstCycle = false;
		}
		else
		{
			TransitionState(next_state);
		}
	}
}

void cRaptorController::UpdateAction()
{
	ParseGround();
	BuildPoliState(mPoliState);

	mIsOffPolicy = true;

	if (HasCommands())
	{
		ProcessCommand(mCurrAction);
	}
	else if (HasNet())
	{
		DecideAction(mCurrAction);
	}
	else if (!IsCurrActionCyclic())
	{
		BuildDefaultAction(mCurrAction);
	}

	ApplyAction(mCurrAction);
}

void cRaptorController::UpdateRBDModel()
{
	Eigen::VectorXd pose;
	Eigen::VectorXd vel;
	mChar->BuildPose(pose);
	mChar->BuildVel(vel);

	mRBDModel->Update(pose, vel);
	cRBDUtil::BuildJacobian(*mRBDModel.get(), mJacobian);
}

void cRaptorController::UpdatePDCtrls(double time_step, Eigen::VectorXd& out_tau)
{
	mImpPDCtrl.UpdateControlForce(time_step, out_tau);
}

void cRaptorController::UpdateStumbleCounter(double time_step)
{
	bool stumbled = mChar->HasStumbled();
	if (stumbled)
	{
		mCurrStumbleCount += time_step;
	}
}

void cRaptorController::UpdateStanceHip()
{
	int stance_hip = GetStanceHip();
	int stance_toe = GetStanceToe();
	bool active = IsActiveVFEffector(stance_toe);
	mImpPDCtrl.GetPDCtrl(stance_hip).SetActive(!active);
}

void cRaptorController::ApplySwingFeedback(Eigen::VectorXd& out_tau)
{
	double cv = GetCv();
	double cd = GetCd();

	bool first_half = mState == eStateContact || mState == eStateDown;
	cd = (first_half) ? 0 : cd;
	cv = (first_half) ? cv : 0;

	tVector com = mChar->CalcCOM();
	tVector com_vel = mChar->CalcCOMVel();

	int stance_toe_id = GetStanceToe();
	int swing_hip_id = GetSwingHip();

	tVector toe_pos = mChar->GetBodyPart(stance_toe_id)->GetPos();
	tVector d = com - toe_pos;
	double d_theta = cd * d[0] + cv * com_vel[0];

	tStateParams params = GetCurrParams();
	double theta0 = params(eStateParamSwingHip);
	double theta_new = theta0 + d_theta;

	mImpPDCtrl.SetTargetTheta(swing_hip_id, theta_new);
}

void cRaptorController::ApplyStanceFeedback(Eigen::VectorXd& out_tau)
{
	const Eigen::MatrixXd& joint_mat = mChar->GetJointMat();

	int stance_toe = GetStanceToe();
	double hip_tau = 0;

	int stance_hip_id = GetStanceHip();
	int swing_hip_id = GetSwingHip();

	const int root_joints[] = 
	{
		cSimRaptor::eJointRightHip,
		cSimRaptor::eJointLeftHip,
		//cSimRaptor::eJointSpine0,
		//cSimRaptor::eJointTail0
	};
	const int num_root_joints = sizeof(root_joints) / sizeof(root_joints[0]);

	if (IsActiveVFEffector(stance_toe))
	{
		for (int j = 0; j < num_root_joints; ++j)
		{
			int curr_id = root_joints[j];
			if (curr_id != stance_hip_id)
			{
				int param_offset = cKinTree::GetParamOffset(joint_mat, curr_id);

				// hack assuming revolute joint
				double curr_tau = out_tau(param_offset);
				hip_tau += -curr_tau;
			}
		}
		
		double target_root_pitch = GetTargetRootPitch();
		double root_pitch = GetRootPitch();
		double root_omega = mChar->GetRootAngVel()[2];

		const cPDController& stance_pd = mImpPDCtrl.GetPDCtrl(stance_hip_id);
		const double kp = stance_pd.GetKp();
		const double kd = stance_pd.GetKd();

		double root_tau = kp * (target_root_pitch - root_pitch) + kd * (-root_omega);
		hip_tau += -root_tau;

		int stance_hip_offset = cKinTree::GetParamOffset(joint_mat, stance_hip_id);
		int stance_hip_size = cKinTree::GetParamSize(joint_mat, stance_hip_id);
		auto stance_tau = out_tau.segment(stance_hip_offset, stance_hip_size);
		stance_tau += hip_tau * Eigen::VectorXd::Ones(stance_hip_size);
	}
}

void cRaptorController::ApplyGravityCompensation(Eigen::VectorXd& out_tau)
{
	const double lambda = 0.0001;
	const Eigen::MatrixXd& joint_mat = mChar->GetJointMat();
	const Eigen::MatrixXd& body_defs = mChar->GetBodyDefs();
	Eigen::VectorXd pose;
	Eigen::VectorXd vel;
	mChar->BuildPose(pose);
	vel = Eigen::VectorXd::Zero(pose.size());

	bool has_support = false;
	Eigen::MatrixXd contact_basis = BuildContactBasis(pose, has_support);
	
	if (has_support)
	{
		Eigen::VectorXd tau_g;
		cRBDUtil::CalcGravityForce(*mRBDModel.get(), tau_g);
		tau_g = -tau_g;

		int basis_cols = static_cast<int>(contact_basis.cols());

		int root_id = mChar->GetRootID();
		int root_offset = mChar->GetParamOffset(root_id);
		int root_size = mChar->GetParamSize(root_id);

		Eigen::VectorXd b = tau_g.segment(root_offset, root_size);
		Eigen::MatrixXd A = contact_basis.block(root_offset, 0, root_size, basis_cols);

		Eigen::VectorXd W(3);
		W(0) = 0.0001;
		W(1) = 0.0001;
		W(2) = 1;

		Eigen::MatrixXd AtA = A.transpose() * W.asDiagonal() * A;
		Eigen::VectorXd Atb = A.transpose() * W.asDiagonal() * b;
		AtA.diagonal() += lambda * Eigen::VectorXd::Ones(basis_cols);

		Eigen::VectorXd x = (AtA).householderQr().solve(Atb);
		Eigen::VectorXd tau_contact = contact_basis * x;

		tau_g -= tau_contact;
		out_tau += tau_g;
	}
}

void cRaptorController::ApplyVirtualForces(Eigen::VectorXd& out_tau)
{
	for (int e = 0; e < gNumEndEffectors; ++e)
	{
		cSimRaptor::eJoint joint_id = gEndEffectors[e];
		bool active_effector = IsActiveVFEffector(joint_id);
		
		int stance_hip_id = GetStanceHip();
		int swing_hip_id = GetSwingHip();
		int root_id = mChar->GetRootID();

		if (active_effector)
		{
			tVector vf = -GetEffectorVF();
			
			tVector pos = GetEndEffectorContactPos(joint_id);
			cSpAlg::tSpTrans joint_world_trans = cSpAlg::BuildTrans(-pos);

			cSpAlg::tSpVec sp_force = cSpAlg::BuildSV(vf);
			sp_force = cSpAlg::ApplyTransF(joint_world_trans, sp_force);

			const Eigen::MatrixXd& joint_mat = mRBDModel->GetJointMat();
			int curr_id = joint_id;
			while (curr_id != root_id)
			{
				assert(curr_id != cKinTree::gInvalidJointID);
				int offset = cKinTree::GetParamOffset(joint_mat, curr_id);
				int size = cKinTree::GetParamSize(joint_mat, curr_id);
				const auto curr_J = mJacobian.block(0, offset, cSpAlg::gSpVecSize, size);

				out_tau.segment(offset, size) += curr_J.transpose() * sp_force;

				if (curr_id == stance_hip_id)
				{
					int swing_offset = cKinTree::GetParamOffset(joint_mat, swing_hip_id);
					int swing_size = cKinTree::GetParamSize(joint_mat, swing_hip_id);

					out_tau.segment(swing_offset, swing_size) += -(curr_J.transpose() * sp_force);
				}

				curr_id = mRBDModel->GetParent(curr_id);
			}
		}
	}
}

const cRaptorController::tStateDef& cRaptorController::GetCurrStateDef() const
{
	return gStateDefs[mState];
}

cRaptorController::tStateParams cRaptorController::GetCurrParams() const
{
	tStateParams params = mCurrAction.mParams.segment(eMiscParamMax + mState * eStateParamMax, eStateParamMax);
	return params;
}

double cRaptorController::GetTargetRootPitch() const
{
	tStateParams params = GetCurrParams();
	double pitch = params(eStateParamRootPitch);
	return pitch;
}

double cRaptorController::GetRootPitch() const
{
	tVector axis = tVector::Zero();
	double theta = 0;
	mChar->GetRootRotation(axis, theta);

	const tVector& ref_axis = tVector(0, 0, 1, 0);
	if (axis.dot(ref_axis) < 0)
	{
		theta = -theta;
	}
	return theta;
}

void cRaptorController::SetStateParams(const tStateParams& params)
{
#if defined(ENABLE_SPINE_CURVE)
	double spine_theta = params(eStateParamSpineCurve);
	for (int i = 0; i < gNumSpineJoints; ++i)
	{
		cSimRaptor::eJoint joint = gSpineJoints[i];
		mImpPDCtrl.SetTargetTheta(joint, spine_theta);
	}
#endif

	mImpPDCtrl.SetTargetTheta(GetStanceHip(), params(eStateParamStanceHip));
	mImpPDCtrl.SetTargetTheta(GetStanceKnee(), params(eStateParamStanceKnee));
	mImpPDCtrl.SetTargetTheta(GetStanceAnkle(), params(eStateParamStanceAnkle));
	mImpPDCtrl.SetTargetTheta(GetSwingHip(), params(eStateParamSwingHip));
	mImpPDCtrl.SetTargetTheta(GetSwingKnee(), params(eStateParamSwingKnee));
	mImpPDCtrl.SetTargetTheta(GetSwingAnkle(), params(eStateParamSwingAnkle));
}

void cRaptorController::SetupPassiveMode()
{
	tStateParams params;
	params.setZero();
	SetStateParams(params);
}

double cRaptorController::GetTransTime() const
{
	return mCurrAction.mParams[eMiscParamTransTime];
}

double cRaptorController::GetCv() const
{
	return mCurrAction.mParams[eMiscParamCv];
}

double cRaptorController::GetCd() const
{
	return mCurrAction.mParams[eMiscParamCd];
}

bool cRaptorController::CheckContact(int joint_id) const
{
	assert(joint_id != cSimRaptor::eJointInvalid);
	const auto& body_part = mChar->GetBodyPart(joint_id);
	bool contact = body_part->IsInContact();
	return contact;
}

bool cRaptorController::IsActiveVFEffector(int joint_id) const
{
	int state = GetState();
	bool valid_effector = (joint_id == GetStanceToe()) && (state == eStateContact || state == eStateDown);
	bool active_effector = valid_effector && CheckContact(joint_id);
	return active_effector;
}

tVector cRaptorController::GetEffectorVF() const
{
	return tVector(mCurrAction.mParams[eMiscParamForceX], mCurrAction.mParams[eMiscParamForceY], 0, 0);
}

Eigen::MatrixXd cRaptorController::BuildContactBasis(const Eigen::VectorXd& pose, bool& out_has_support) const
{
	const int num_basis = 2;
	int rows = static_cast<int>(pose.size());
	const int cols = gNumEndEffectors * num_basis;

	const double cone_theta = M_PI * 0.1;
	double s = std::sin(cone_theta);
	double c = std::cos(cone_theta);

	const Eigen::Matrix<double, 6, num_basis> force_svs
		((Eigen::Matrix<double, 6, num_basis>() << 
			0, 0,
			0, 0,
			0, 0,
			0, 1,
			1, 0,
			0, 0).finished());

	Eigen::MatrixXd contact_basis = Eigen::MatrixXd::Zero(rows, cols);

	const Eigen::MatrixXd& joint_mat = mChar->GetJointMat();
	tVector root_pos = mChar->GetRootPos();

	out_has_support = false;
	for (int e = 0; e < gNumEndEffectors; ++e)
	{
		cSimRaptor::eJoint joint_id = gEndEffectors[e];
		bool valid_support = IsActiveVFEffector(joint_id);
		//bool valid_support = CheckContact(joint_id);

		auto curr_block = contact_basis.block(0, e * num_basis, rows, num_basis);
		if (valid_support)
		{
			out_has_support = true;

			tVector pos = GetEndEffectorContactPos(joint_id);
			cSpAlg::tSpTrans joint_world_trans = cSpAlg::BuildTrans(-pos);

			Eigen::Matrix<double, 6, num_basis> force_basis;
			for (int i = 0; i < num_basis; ++i)
			{
				cSpAlg::tSpVec force_sv = force_svs.col(i);
				force_basis.col(i) = cSpAlg::ApplyTransF(joint_world_trans, force_sv);
			}

			int curr_id = joint_id;
			while (curr_id != cKinTree::gInvalidJointID)
			{
				int offset = cKinTree::GetParamOffset(joint_mat, curr_id);
				int size = cKinTree::GetParamSize(joint_mat, curr_id);
				const auto curr_J = mJacobian.block(0, offset, cSpAlg::gSpVecSize, size);

				curr_block.block(offset, 0, size, curr_block.cols()) = curr_J.transpose() * force_basis;
				curr_id = mRBDModel->GetParent(curr_id);
			}
		}
	}

	return contact_basis;
}

const std::string& cRaptorController::GetStateName(eState state) const
{
	return gStateDefs[state].mName;
}

const std::string& cRaptorController::GetStateParamName(eStateParam param) const
{
	return gStateParamNames[param];
}

void cRaptorController::GetOptParams(const Eigen::VectorXd& ctrl_params, Eigen::VectorXd& out_opt_params) const
{
	int num_params = GetNumParams();
	int num_opt_params = GetNumOptParams();
	assert(ctrl_params.size() == num_params);
	assert(gNumOptParamMasks == num_params);

	out_opt_params.resize(num_opt_params);
	
	int opt_idx = 0;
	for (int i = 0; i < num_params; ++i)
	{
		if (IsOptParam(i))
		{
			out_opt_params[opt_idx] = ctrl_params[i];
			++opt_idx;
		}
	}
	assert(opt_idx == num_opt_params);
}

std::string cRaptorController::BuildOptParamsJson(const Eigen::VectorXd& opt_params) const
{
	assert(opt_params.size() == GetNumOptParams());
	Eigen::VectorXd param_buffer = mCurrAction.mParams;
	SetOptParams(opt_params, param_buffer);

	std::string json = "";
	int idx = 0;

	json += "\"" + gMiscParamsKey + "\": \n{\n";
	for (int i = 0; i < eMiscParamMax; ++i)
	{
		if (i != 0)
		{
			json += ",\n";
		}
		json += "\"" + gMiscParamsNames[i] +  "\": " + std::to_string(param_buffer[idx++]);
	}
	json += "\n},\n\n";

	json += "\"" + gStateParamsKey + "\": \n{\n";
	for (int i = 0; i < eStateMax; ++i)
	{
		if (i != 0)
		{
			json += ",\n";
		}

		json += "\"" + gStateDefs[i].mName + "\": \n{\n";
		for (int j = 0; j < eStateParamMax; ++j)
		{
			if (j != 0)
			{
				json += ",\n";
			}
			json += "\"" + gStateParamNames[j] + "\": " + std::to_string(param_buffer[idx++]);
		}
		json += "\n}";
	}
	json += "\n}\n";

	json = "{" + json + "}";
	return json;
}

void cRaptorController::BuildStateParamsFromPose(const Eigen::VectorXd& pose, tStateParams& out_params)
{
	const cSimRaptor::eJoint ctrl_joints[] =
	{
		cSimRaptor::eJointRightHip,
		cSimRaptor::eJointRightKnee,
		cSimRaptor::eJointRightAnkle,
		cSimRaptor::eJointLeftHip,
		cSimRaptor::eJointLeftKnee,
		cSimRaptor::eJointLeftAnkle,
	};
	const int num_ctrl_joints = sizeof(ctrl_joints) / sizeof(ctrl_joints[0]);

	const Eigen::MatrixXd& joint_mat = mChar->GetJointMat();
	
	double root_theta = cKinTree::GetRootTheta(joint_mat, pose);
	out_params(eStateParamRootPitch) = root_theta;
	out_params(eStateParamSpineCurve) = CalcSpineCurve(pose);

	for (int i = 0; i < num_ctrl_joints; ++i)
	{
		tVector axis;
		double tar_theta = 0;
		int joint_id = ctrl_joints[i];
		if (mImpPDCtrl.UseWorldCoord(joint_id))
		{
			cKinTree::CalcJointWorldTheta(joint_mat, pose, joint_id, axis, tar_theta);
		}
		else
		{
			tar_theta = cKinTree::GetJointTheta(joint_mat, pose, joint_id);
		}
		out_params(eStateParamStanceHip + i) = tar_theta;
	}
}

double cRaptorController::CalcSpineCurve(const Eigen::VectorXd& pose) const
{
	const Eigen::MatrixXd& joint_mat = mChar->GetJointMat();

	double curve = 0;
	for (int i = 0; i < gNumSpineJoints; ++i)
	{
		cSimRaptor::eJoint joint = gSpineJoints[i];
		double theta = cKinTree::GetJointTheta(joint_mat, pose, joint);
		curve += theta;
	}

	curve /= gNumSpineJoints;
	return curve;
}

const Eigen::VectorXd& cRaptorController::GetCtrlParams(int ctrl_id) const
{
	return mCtrlParams[ctrl_id];
}

bool cRaptorController::IsCurrActionCyclic() const
{
	const tBlendAction& action = mActions[mCurrAction.mID];
	return action.mCyclic;
}

void cRaptorController::ApplyAction(int action_id)
{
	cTerrainRLCharController::ApplyAction(action_id);
}

void cRaptorController::ApplyAction(const tAction& action)
{
	cTerrainRLCharController::ApplyAction(action);
	TransitionState(eStateContact);
}

void cRaptorController::NewCycleUpdate()
{
	cTerrainRLCharController::NewCycleUpdate();
	mPrevCycleTime = mCurrCycleTime;
	mCurrCycleTime = 0;
	mPrevStumbleCount = mCurrStumbleCount;
	mCurrStumbleCount = 0;
	RecordDistTraveled();
	mPrevCOM = mChar->CalcCOM();
}

void cRaptorController::BlendCtrlParams(const tBlendAction& action, Eigen::VectorXd& out_params) const
{
	const Eigen::VectorXd& param0 = GetCtrlParams(action.mParamIdx0);
	const Eigen::VectorXd& param1 = GetCtrlParams(action.mParamIdx1);
	double blend = action.mBlend;
	out_params = (1 - blend) * param0 + blend * param1;
}

void cRaptorController::PostProcessParams(Eigen::VectorXd& out_params) const
{
	out_params[eMiscParamTransTime] = std::abs(out_params[eMiscParamTransTime]);
	out_params[eMiscParamCv] = std::abs(out_params[eMiscParamCv]);
	out_params[eMiscParamCd] = std::abs(out_params[eMiscParamCd]);
}

bool cRaptorController::IsOptParam(int param_idx) const
{
	assert(param_idx >= 0 && param_idx < gNumOptParamMasks);
	return gOptParamsMasks[param_idx];
}

void cRaptorController::BuildPoliStatePose(Eigen::VectorXd& out_pose) const
{
	cTerrainRLCharController::BuildPoliStatePose(out_pose);
	eStance stance = GetStance();
	if (stance != gDefaultStance)
	{
		FlipPoliPoseStance(out_pose);
	}
}

void cRaptorController::BuildPoliStateVel(Eigen::VectorXd& out_vel) const
{
	cTerrainRLCharController::BuildPoliStateVel(out_vel);
	eStance stance = GetStance();
	if (stance != gDefaultStance)
	{
		FlipPoliPoseStance(out_vel);
	}
}

void cRaptorController::FlipStance()
{
	SetStance((mStance == eStanceRight) ? eStanceLeft : eStanceRight);
}

void cRaptorController::SetStance(eStance stance)
{
	mStance = stance;
	mImpPDCtrl.GetPDCtrl(GetStanceHip()).SetActive(false);
	mImpPDCtrl.GetPDCtrl(GetSwingHip()).SetActive(true);

	TransitionState(GetState(), GetPhase());
}

void cRaptorController::FlipPoseStance(Eigen::VectorXd& out_pose) const
{
	const Eigen::MatrixXd& joint_mat = mChar->GetJointMat();

	int num_dofs = mChar->GetNumDof();
	int last_body_joint = cSimRaptor::eJointTail4;
	int start_idx = cKinTree::GetParamOffset(joint_mat, last_body_joint);
	start_idx += cKinTree::GetParamSize(joint_mat, last_body_joint);

	int num_leg_params = (num_dofs - start_idx) / 2;
	for (int i = 0; i < num_leg_params; ++i)
	{
		int idx0 = start_idx + i;
		int idx1 = start_idx + num_leg_params + i;
		double val0 = out_pose(idx0);
		double val1 = out_pose(idx1);
		out_pose(idx0) = val1;
		out_pose(idx1) = val0;
	}
}

void cRaptorController::FlipPoliPoseStance(Eigen::VectorXd& out_pose) const
{
	// hack
	// assume that the leg params are all packed at the end of the vector!
	const Eigen::MatrixXd& joint_mat = mChar->GetJointMat();

	int num_params = static_cast<int>(out_pose.size());
	int num_leg_joints = GetNumJointsPerLeg();
	int num_leg_params = num_leg_joints * gPosDim;

	for (int i = 0; i < num_leg_params; ++i)
	{
		int idx0 = num_params - i - 1;
		int idx1 = num_params - num_leg_params - i - 1;
		double val0 = out_pose(idx0);
		double val1 = out_pose(idx1);
		out_pose(idx0) = val1;
		out_pose(idx1) = val0;
	}
}

int cRaptorController::GetNumJointsPerLeg() const
{
	int hip_id = GetStanceHip();
	int toe_id = GetStanceToe();
	int num_joints = std::abs(toe_id - hip_id) + 1;
	return num_joints;
}

int cRaptorController::GetSwingHip() const
{
	return (GetStance() == eStanceRight) ? cSimRaptor::eJointLeftHip : cSimRaptor::eJointRightHip;
}

int cRaptorController::GetSwingKnee() const
{
	return (GetStance() == eStanceRight) ? cSimRaptor::eJointLeftKnee : cSimRaptor::eJointRightKnee;
}

int cRaptorController::GetSwingAnkle() const
{
	return (GetStance() == eStanceRight) ? cSimRaptor::eJointLeftAnkle : cSimRaptor::eJointRightAnkle;
}

int cRaptorController::GetSwingToe() const
{
	return (GetStance() == eStanceRight) ? cSimRaptor::eJointLeftToe : cSimRaptor::eJointRightToe;
}

int cRaptorController::GetStanceHip() const
{
	return (GetStance() == eStanceRight) ? cSimRaptor::eJointRightHip : cSimRaptor::eJointLeftHip;
}

int cRaptorController::GetStanceKnee() const
{
	return (GetStance() == eStanceRight) ? cSimRaptor::eJointRightKnee : cSimRaptor::eJointLeftKnee;
}

int cRaptorController::GetStanceAnkle() const
{
	return (GetStance() == eStanceRight) ? cSimRaptor::eJointRightAnkle : cSimRaptor::eJointLeftAnkle;
}

int cRaptorController::GetStanceToe() const
{
	return (GetStance() == eStanceRight) ? cSimRaptor::eJointRightToe : cSimRaptor::eJointLeftToe;
}

tVector cRaptorController::GetEndEffectorContactPos(int joint_id) const
{
	tVector pos = tVector::Zero();
	const auto& part = mChar->GetBodyPart(joint_id);
	if (part != nullptr)
	{
		const auto& body_defs = mChar->GetBodyDefs();
		tVector size = cKinTree::GetBodySize(body_defs, joint_id);
		pos = part->LocalToWorldPos(tVector(0, -0.5 * size[1], 0, 0));
	}
	else
	{
		pos = mRBDModel->CalcJointWorldPos(joint_id);
	}
	return pos;
}

void cRaptorController::RecordDistTraveled()
{
	tVector com = mChar->CalcCOM();
	mPrevDistTraveled = com - mPrevCOM;
}

int cRaptorController::PopCommand()
{
	int cmd = mCommands.top();
	mCommands.pop();
	return cmd;
}

bool cRaptorController::HasCommands() const
{
	return !mCommands.empty();
}

void cRaptorController::ClearCommands()
{
	while (!mCommands.empty())
	{
		mCommands.pop();
	}
}

void cRaptorController::ProcessCommand(tAction& out_action)
{
	int a_id = PopCommand();
	BuildBaseAction(a_id, out_action);
}

void cRaptorController::BuildBaseAction(int action_id, tAction& out_action) const
{
	const tBlendAction& blend = mActions[action_id];
	out_action.mID = blend.mID;
	BlendCtrlParams(blend, out_action.mParams);
}
back to top