DynaMechs/CustomTaskSpaceController.h at master · yiping/DynaMechs · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
/*
 *  CustomTaskSpaceController.h
 *  DynaMechs
 *
 *  Created by Patrick Wensing on 6/12/12.
 *  Copyright 2012 __MyCompanyName__. All rights reserved.
 *
 */

#ifndef __CUSTOM_TASK_SPACE_CONTROLLER_H__
#define __CUSTOM_TASK_SPACE_CONTROLLER_H__


#include "TaskSpaceController.h"
#include "mosek.h"
#include <Eigen/Core>
#include "dmArticulation.hpp"

typedef vector<int> IntVector;
typedef vector<MatrixX6F> XformVector;
class CustomTaskSpaceController : public TaskSpaceController
{
public:
	CustomTaskSpaceController(dmArticulation * art);

	// This function
	void ObtainArticulationData();

	void AssignFootMaxLoad(int index, double maxLoad);

	void UpdateObjective();
	void UpdateTauObjective();

	void UpdateVariableBounds();

	void UpdateConstraintMatrix();
	void UpdateInitialConstraintBounds();
	void UpdateHPTConstraintBounds();

	void Optimize();


	Float norm(const VectorXF & a,const VectorXF & b);
	void logBarrier(const VectorXF & x, Float &b, VectorXF & bs);
	void evalBarrier(const VectorXF & x, bool computeHess, VectorXF & bs, VectorXF& grad, MatrixXF& H, MatrixXF & iH);
	void evalResidual(const VectorXF & x, const VectorXF & v, const MatrixXF & Aeq, const VectorXF & beq, const Float t, bool computeHess, VectorXF & rd, VectorXF & rp, VectorXF & bs, VectorXF& grad, MatrixXF & H, MatrixXF & iH);


	MatrixXF LambdaInvTau,LambdaInvF;
	MatrixXF Lttau,Ltf, Lctau,Lcf;
	VectorXF bt, bc;
	VectorXF tauLb,tauUb;
	VectorXF fnetUb;
	VectorXF barriers, barriersHat;
	vector<VectorXF> grads, gradsHat;
	vector<Eigen::DiagonalMatrix<Float, Dynamic, Dynamic> > Hs, HsHat, iHs;
	VectorXF grad, gradHat;
	VectorXF rdInit;

	int numConstraintTasks, numOptimTasks, fStart, eStart, zStart;

private:
	int fLen, vLen, xLen, barLen;

	VectorXF eBiasCandG;
	LDLT<MatrixXF> Hdecomp;
};

#endif