Variables
	com_ref = robot.com(data)

	comTask = tsid.TaskComEquality("task-com", robot)

float	contact_Point = np.ones((3, 4)) * lz

float	CONTACT_TRANSITION_TIME = 1.0

	contactLF

	contactNormal = np.array([0.0, 0.0, 1.0])

	contactRF

	data = invdyn.data()

float	dt = 0.01
	Test 1 ##################3. More...

	dv = invdyn.getAccelerations(sol)

	f = invdyn.getContactForce(contactRF.name, sol)

	filename = str(os.path.dirname(os.path.abspath(__file__)))

float	fMax = 1000.0

float	fMin = 5.0

	H_lf_ref = robot.position(data, robot.model().getJointId(lf_frame_name))

	H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))

	H_rf_ref_vec = np.zeros(12)

	HQPData = invdyn.computeProblemData(t, q, v)

	invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)

float	kp_com = 30.0

float	kp_contact = 100.0

float	kp_posture = 30.0

float	kp_RF = 100.0

string	lf_frame_name = "LAnkleRoll"

float	lxn = 0.077

float	lxp = 0.14

float	lyn = 0.069

float	lyp = 0.069

float	lz = 0.105

int	max_it = 1000

	model = robot.model()

float	mu = 0.3

string	path = filename + "/../../models/romeo"

	postureTask = tsid.TaskJointPosture("task-posture", robot)

int	PRINT_N = 100

	q = model.referenceConfigurations["half_sitting"]

	q_ref = q[7:]

int	REMOVE_CONTACT_N = 100

string	rf_frame_name = "RAnkleRoll"

	rightFootTask = tsid.TaskSE3Equality("task-right-foot", robot, rf_frame_name)

	robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)

	s = tsid.TrajectorySample(12, 6)

	sampleCom = tsid.TrajectorySample(3)

	samplePosture = tsid.TrajectorySample(robot.nv - 6)

	sol = solver.solve(HQPData)

	solver = tsid.SolverHQuadProg("qp solver")

string	srdf = path + "/srdf/romeo_collision.srdf"

float	t = 0.0

	tau = invdyn.getActuatorForces(sol)

	tau_old = np.zeros(robot.nv - 6)

	trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)

	trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)

string	urdf = path + "/urdf/romeo.urdf"

	v = np.zeros(robot.nv)

	vector = se3.StdVec_StdString()

float	w_com = 1.0

int	w_forceRef = 1e-5

int	w_posture = 1e-2

int	w_RF = 1e3

Variable Documentation

◆ com_ref

test_Formulation.com_ref = robot.com(data)

Definition at line 123 of file test_Formulation.py.

◆ comTask

test_Formulation.comTask = tsid.TaskComEquality("task-com", robot)

Definition at line 91 of file test_Formulation.py.

◆ contact_Point

float test_Formulation.contact_Point = np.ones((3, 4)) * lz

Definition at line 53 of file test_Formulation.py.

◆ CONTACT_TRANSITION_TIME

float test_Formulation.CONTACT_TRANSITION_TIME = 1.0

Definition at line 105 of file test_Formulation.py.

◆ contactLF

test_Formulation.contactLF

Initial value:

 =  tsid.Contact6d(
     "contact_lfoot",
     robot,
     lf_frame_name,
     contact_Point,
     contactNormal,
     mu,
     fMin,
     fMax,
     w_forceRef,
 )

Definition at line 74 of file test_Formulation.py.

◆ contactNormal

test_Formulation.contactNormal = np.array([0.0, 0.0, 1.0])

Definition at line 39 of file test_Formulation.py.

◆ contactRF

test_Formulation.contactRF

Initial value:

 =  tsid.Contact6d(
     "contact_rfoot",
     robot,
     rf_frame_name,
     contact_Point,
     contactNormal,
     mu,
     fMin,
     fMax,
     w_forceRef,
 )

Definition at line 57 of file test_Formulation.py.

◆ data

test_Formulation.data = invdyn.data()

Definition at line 52 of file test_Formulation.py.

◆ dt

float test_Formulation.dt = 0.01

Test 1 ##################3.

Definition at line 102 of file test_Formulation.py.

◆ dv

test_Formulation.dv = invdyn.getAccelerations(sol)

Definition at line 153 of file test_Formulation.py.

◆ f

test_Formulation.f = invdyn.getContactForce(contactRF.name, sol)

Definition at line 161 of file test_Formulation.py.

◆ filename

test_Formulation.filename = str(os.path.dirname(os.path.abspath(__file__)))

Definition at line 12 of file test_Formulation.py.

◆ fMax

float test_Formulation.fMax = 1000.0

Definition at line 36 of file test_Formulation.py.

◆ fMin

float test_Formulation.fMin = 5.0

Definition at line 35 of file test_Formulation.py.

◆ H_lf_ref

test_Formulation.H_lf_ref = robot.position(data, robot.model().getJointId(lf_frame_name))

Definition at line 87 of file test_Formulation.py.

◆ H_rf_ref

test_Formulation.H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))

Definition at line 70 of file test_Formulation.py.

◆ H_rf_ref_vec

test_Formulation.H_rf_ref_vec = np.zeros(12)

Definition at line 116 of file test_Formulation.py.

◆ HQPData

test_Formulation.HQPData = invdyn.computeProblemData(t, q, v)

Definition at line 147 of file test_Formulation.py.

◆ invdyn

test_Formulation.invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)

Definition at line 50 of file test_Formulation.py.

◆ kp_com

float test_Formulation.kp_com = 30.0

Definition at line 44 of file test_Formulation.py.

◆ kp_contact

float test_Formulation.kp_contact = 100.0

Definition at line 43 of file test_Formulation.py.

◆ kp_posture

float test_Formulation.kp_posture = 30.0

Definition at line 45 of file test_Formulation.py.

◆ kp_RF

float test_Formulation.kp_RF = 100.0

Definition at line 106 of file test_Formulation.py.

◆ lf_frame_name

string test_Formulation.lf_frame_name = "LAnkleRoll"

Definition at line 38 of file test_Formulation.py.

◆ lxn

float test_Formulation.lxn = 0.077

Definition at line 30 of file test_Formulation.py.

◆ lxp

float test_Formulation.lxp = 0.14

Definition at line 29 of file test_Formulation.py.

◆ lyn

float test_Formulation.lyn = 0.069

Definition at line 32 of file test_Formulation.py.

◆ lyp

float test_Formulation.lyp = 0.069

Definition at line 31 of file test_Formulation.py.

◆ lz

float test_Formulation.lz = 0.105

Definition at line 33 of file test_Formulation.py.

◆ max_it

int test_Formulation.max_it = 1000

Definition at line 108 of file test_Formulation.py.

◆ model

test_Formulation.model = robot.model()

Definition at line 21 of file test_Formulation.py.

◆ mu

float test_Formulation.mu = 0.3

Definition at line 34 of file test_Formulation.py.

◆ path

string test_Formulation.path = filename + "/../../models/romeo"

Definition at line 13 of file test_Formulation.py.

◆ postureTask

test_Formulation.postureTask = tsid.TaskJointPosture("task-posture", robot)

Definition at line 96 of file test_Formulation.py.

◆ PRINT_N

int test_Formulation.PRINT_N = 100

Definition at line 103 of file test_Formulation.py.

◆ q

test_Formulation.q = model.referenceConfigurations["half_sitting"]

Definition at line 23 of file test_Formulation.py.

◆ q_ref

test_Formulation.q_ref = q[7:]

Definition at line 128 of file test_Formulation.py.

◆ REMOVE_CONTACT_N

int test_Formulation.REMOVE_CONTACT_N = 100

Definition at line 104 of file test_Formulation.py.

◆ rf_frame_name

string test_Formulation.rf_frame_name = "RAnkleRoll"

Definition at line 37 of file test_Formulation.py.

◆ rightFootTask

test_Formulation.rightFootTask = tsid.TaskSE3Equality("task-right-foot", robot, rf_frame_name)

Definition at line 109 of file test_Formulation.py.

◆ robot

test_Formulation.robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)

Definition at line 17 of file test_Formulation.py.

◆ s

test_Formulation.s = tsid.TrajectorySample(12, 6)

Definition at line 115 of file test_Formulation.py.

◆ sampleCom

test_Formulation.sampleCom = tsid.TrajectorySample(3)

Definition at line 126 of file test_Formulation.py.

◆ samplePosture

test_Formulation.samplePosture = tsid.TrajectorySample(robot.nv - 6)

Definition at line 130 of file test_Formulation.py.

◆ sol

test_Formulation.sol = solver.solve(HQPData)

Definition at line 151 of file test_Formulation.py.

◆ solver

test_Formulation.solver = tsid.SolverHQuadProg("qp solver")

Definition at line 132 of file test_Formulation.py.

◆ srdf

string test_Formulation.srdf = path + "/srdf/romeo_collision.srdf"

Definition at line 19 of file test_Formulation.py.

◆ t

float test_Formulation.t = 0.0

Definition at line 28 of file test_Formulation.py.

◆ tau

test_Formulation.tau = invdyn.getActuatorForces(sol)

Definition at line 152 of file test_Formulation.py.

◆ tau_old

test_Formulation.tau_old = np.zeros(robot.nv - 6)

Definition at line 135 of file test_Formulation.py.

◆ trajCom

test_Formulation.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)

Definition at line 125 of file test_Formulation.py.

◆ trajPosture

test_Formulation.trajPosture = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)

Definition at line 129 of file test_Formulation.py.

◆ urdf

string test_Formulation.urdf = path + "/urdf/romeo.urdf"

Definition at line 14 of file test_Formulation.py.

◆ v

test_Formulation.v = np.zeros(robot.nv)

Definition at line 26 of file test_Formulation.py.

◆ vector

test_Formulation.vector = se3.StdVec_StdString()

Definition at line 15 of file test_Formulation.py.

◆ w_com

float test_Formulation.w_com = 1.0

Definition at line 40 of file test_Formulation.py.

◆ w_forceRef

int test_Formulation.w_forceRef = 1e-5

Definition at line 42 of file test_Formulation.py.

◆ w_posture

int test_Formulation.w_posture = 1e-2

Definition at line 41 of file test_Formulation.py.

◆ w_RF

int test_Formulation.w_RF = 1e3

Definition at line 107 of file test_Formulation.py.

Variables