Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
HQP
Commits
d17547ab
Commit
d17547ab
authored
Sep 30, 2017
by
Galo Maldonado
Browse files
update
parent
99f36c37
Changes
8
Expand all
Hide whitespace changes
Inline
Side-by-side
hqp/#algebra.py#
0 → 100644
View file @
d17547ab
def quaternion_from_matrix(matrix, isprecise=False):
"""Return quaternion from rotation matrix.
If isprecise is True, the input matrix is assumed to be a precise rotation
matrix and a faster algorithm is used.
>>> q = quaternion_from_matrix(np.identity(4), True)
>>> np.allclose(q, [1, 0, 0, 0])
True
>>> q = quaternion_from_matrix(np.diag([1, -1, -1, 1]))
>>> np.allclose(q, [0, 1, 0, 0]) or np.allclose(q, [0, -1, 0, 0])
True
>>> R = rotation_matrix(0.123, (1, 2, 3))
>>> q = quaternion_from_matrix(R, True)
>>> np.allclose(q, [0.9981095, 0.0164262, 0.0328524, 0.0492786])
True
>>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0],
... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]]
>>> q = quaternion_from_matrix(R)
>>> np.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611])
True
>>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0],
... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]]
>>> q = quaternion_from_matrix(R)
>>> np.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603])
hqp/#wrapper.py#
0 → 100644
View file @
d17547ab
This diff is collapsed.
Click to expand it.
hqp/inv_dyn_formulation_util.py
View file @
d17547ab
...
...
@@ -2,7 +2,7 @@ import numpy as np
from
numpy.linalg
import
norm
from
numpy.random
import
random
from
polytope_conversion_utils
import
cone_span_to_face
from
wrapper
import
Wrapper
from
hqp.
wrapper
import
Wrapper
import
pinocchio
as
se3
from
pinocchio.utils
import
zero
as
zeros
from
acc_bounds_util_multi_dof
import
computeAccLimits
...
...
@@ -195,9 +195,9 @@ class InvDynFormulation (object):
def
__init__
(
self
,
name
,
q
,
v
,
dt
,
mesh_dir
,
urdfFileName
,
freeFlyer
=
True
):
if
(
freeFlyer
):
self
.
r
=
Robot
Wrapper
(
urdfFileName
,
mesh_dir
,
root_joint
=
se3
.
JointModelFreeFlyer
());
self
.
r
=
Wrapper
(
urdfFileName
,
mesh_dir
,
'robot'
,
True
)
#
root_joint=se3.JointModelFreeFlyer());
else
:
self
.
r
=
Robot
Wrapper
(
urdfFileName
,
mesh_dir
,
None
);
self
.
r
=
Wrapper
(
urdfFileName
,
mesh_dir
,
None
);
self
.
freeFlyer
=
freeFlyer
;
self
.
nq
=
self
.
r
.
nq
;
self
.
nv
=
self
.
r
.
nv
;
...
...
@@ -404,7 +404,8 @@ class InvDynFormulation (object):
self
.
setPositions
(
q
,
updateConstraintReference
=
False
);
self
.
setVelocities
(
v
);
self
.
r
.
computeAllTerms
(
q
,
v
);
#self.r.computeAllTerms(q, v);
self
.
r
.
update
(
q
);
self
.
r
.
framesKinematics
(
q
);
self
.
x_com
=
self
.
r
.
com
(
q
,
update_kinematics
=
False
);
self
.
J_com
=
self
.
r
.
Jcom
(
q
,
update_kinematics
=
False
);
...
...
hqp/simulator.py
View file @
d17547ab
...
...
@@ -11,6 +11,105 @@ class Simulator(object):
COM_SPHERE_RADIUS
=
0.01
COM_SPHERE_COLOR
=
(
1
,
0
,
0
,
1
)
'''
name = ''
q = None; # current positions
v = None; # current velocities
LCP = None; # approximate LCP solver using staggered projections
USE_LCP_SOLVER = True;
DETECT_CONTACT_POINTS = True;
#'' True: detect collisions between feet and ground, False: collision is specified by the user ''
GROUND_HEIGHT = 0.0;
LOW_PASS_FILTER_INPUT_TORQUES = False;
ENABLE_FORCE_LIMITS = True;
ENABLE_TORQUE_LIMITS = True;
ENABLE_JOINT_LIMITS = True;
ACCOUNT_FOR_ROTOR_INERTIAS = False;
VIEWER_DT = 0.05;
DISPLAY_COM = True;
DISPLAY_CAPTURE_POINT = True;
COM_SPHERE_RADIUS = 0.01;
CAPTURE_POINT_SPHERE_RADIUS = 0.01;
CONTACT_FORCE_ARROW_RADIUS = 0.01;
COM_SPHERE_COLOR = (1, 0, 0, 1); # red, green, blue, alpha
CAPTURE_POINT_SPHERE_COLOR = (0, 1, 0, 1);
CONTACT_FORCE_ARROW_COLOR = (1, 0, 0, 1);
CONTACT_FORCE_ARROW_SCALE = 1e-3;
contact_force_arrow_names = []; # list of names of contact force arrows
SLIP_VEL_THR = 0.1;
SLIP_ANGVEL_THR = 0.2;
NORMAL_FORCE_THR = 5.0;
JOINT_LIMITS_DQ_THR = 1e-1; #1.0;
TORQUE_VIOLATION_THR = 1.0;
DQ_MAX = 9.14286;
ENABLE_WALL_DRILL_CONTACT = False;
wall_x = 0.5;
wall_damping = np.array([30, 30, 30, 0.3, 0.3, 0.3]);
k=0; # number of contact constraints (i.e. size of contact force vector)
na=0; # number of actuated DoFs
nq=0; # number of position DoFs
nv=0; # number of velocity DoFs
r=[]; # robot
mu=[]; # friction coefficient (force, moment)
fMin = 0; # minimum normal force
dt = 0; # time step used to compute joint acceleration bounds
qMin = []; # joint lower bounds
qMax = []; # joint upper bounds
#'' Mapping between y and tau: y = C*tau+c ''
C = [];
c = [];
M = []; # mass matrix
Md = []; # rotor inertia
h = []; # dynamic drift
q = [];
dq = [];
x_com = []; # com 3d position
dx_com = []; # com 3d velocity
ddx_com = []; # com 3d acceleration
cp = None; # capture point
# H_lankle = []; # left ankle homogeneous matrix
# H_rankle = []; # right ankle homogeneous matrix
J_com = []; # com Jacobian
Jc = []; # contact Jacobian
Minv = []; # inverse of the mass matrix
Jc_Minv = []; # Jc*Minv
Lambda_c = []; # task-space mass matrix (Jc*Minv*Jc^T)^-1
Jc_T_pinv = []; # Lambda_c*Jc_Minv
Nc_T = []; # I - Jc^T*Jc_T_pinv
S_T = []; # selection matrix
dJc_v = []; # product of contact Jacobian time derivative and velocity vector: dJc*v
candidateContactConstraints = [];
rigidContactConstraints = [];
# constr_rfoot = None;
# constr_lfoot = None;
# constr_rhand = None;
# f_rh = []; # force right hand
# w_rf = []; # wrench right foot
# w_lf = []; # wrench left foot
#'' debug variables ''
x_c = []; # contact points
dx_c = []; # contact points velocities
x_c_init = []; # initial position of constrained bodies
viewer = None;
'''
def
reset
(
self
,
t
,
q
,
v
,
dt
,
nv
):
n
=
nv
self
.
robot
.
dt
=
0.0025
#dt
...
...
@@ -31,9 +130,11 @@ class Simulator(object):
self
.
viewer
=
Viewer
(
self
.
robot
.
name
,
self
.
robot
)
self
.
updateRobotConfig
(
self
.
robot
.
q0
)
self
.
viewer
.
display
(
self
.
robot
.
q0
,
robot
.
name
)
#self.fMin = 0.001
#self.mu = np.array([ 0.3, 0.1])
#self.nq = self.robot.nq
#self.nv = self.robot.nv
#self.na = self.nv
-6
#self.na = self.nv
#if(self.DISPLAYCOM):
# self.viewer.addSphere('com', self.COM_SPHERE_RADIUS, mat_zeros(3), mat_zeros(3), self.COM_SPHERE_COLOR, 'OFF')
...
...
hqp/solvers.py
View file @
d17547ab
import
numpy
as
np
from
wrapper
import
Wrapper
import
scipy
from
abstract_solver
import
AbstractSolver
class
NProjections
():
def
reset
(
self
,
q
,
v
,
dt
):
...
...
@@ -166,3 +166,34 @@ class NProjections():
Z
+=
self
.
null
(
Jstack
[
k
]
/
robot
.
M
)
*
Z
[
k
-
1
]
return
Z
class
StandardQpSolver
(
AbstractSolver
):
"""
Nonrobust inverse dynamics solver for the problem:
min ||D*x - d||^2
s.t. lbA <= A*x <= ubA
"""
def
__init__
(
self
,
n
,
m_in
,
solver
=
'slsqp'
,
accuracy
=
1e-6
,
maxIter
=
100
,
verb
=
0
):
AbstractSolver
.
__init__
(
self
,
n
,
m_in
,
solver
,
accuracy
,
maxIter
,
verb
);
self
.
name
=
"Classic TSID"
;
def
f_cost
(
self
,
x
):
e
=
np
.
dot
(
self
.
D
,
x
)
-
self
.
d
;
return
0.5
*
np
.
dot
(
e
.
T
,
e
);
def
f_cost_grad
(
self
,
x
):
return
np
.
dot
(
self
.
H
,
x
)
-
self
.
dD
;
def
f_cost_hess
(
self
,
x
):
return
self
.
H
;
def
get_linear_inequality_matrix
(
self
):
return
self
.
A
;
def
get_linear_inequality_vectors
(
self
):
return
(
self
.
lbA
,
self
.
ubA
);
hqp/tasks.py
View file @
d17547ab
...
...
@@ -60,6 +60,7 @@ class SE3Task(Task):
self
.
_mask
=
(
np
.
ones
(
6
)).
astype
(
bool
)
# for local to global
self
.
_gMl
=
SE3
.
Identity
()
self
.
__gain_matrix
=
np
.
matrix
(
np
.
eye
(
robot
.
nv
))
def
mask
(
self
,
mask
):
assert
len
(
mask
)
==
6
,
"The mask must have 6 elemets"
...
...
@@ -73,6 +74,10 @@ class SE3Task(Task):
@
property
def
refTrajectory
(
self
):
return
self
.
_ref_trajectory
def
setGain
(
self
,
gain_vector
):
assert
gain_vector
.
shape
==
(
self
.
robot
.
nv
,)
self
.
__gain_matrix
=
np
.
matrix
(
np
.
diag
(
gain_vector
))
def
jointPostition
(
self
):
return
robot
.
position
(
robot
.
q
,
joint_id
)
...
...
@@ -138,19 +143,19 @@ class SE3Task(Task):
#_ Taks functions:
# Compute error acceleration desired
p_error
=
errorInSE3
(
oMi
,
M_ref
);
v_error
=
v_frame
-
self
.
_gMl
.
actInv
(
v_ref
)
self
.
p_error
=
errorInSE3
(
oMi
,
M_ref
);
self
.
v_error
=
v_frame
-
self
.
_gMl
.
actInv
(
v_ref
)
drift
=
self
.
robot
.
frameAcceleration
(
self
.
_frame_id
)
drift
.
linear
+=
np
.
cross
(
v_frame
.
angular
.
T
,
v_frame
.
linear
.
T
).
T
# porportional derivative task
if
self
.
expDecay
is
True
:
self
.
kv
=
exponentialDecay
(
self
.
kp
)
if
self
.
adaptGain
is
True
:
self
.
kp
=
adaptativeGain
(
p_error
.
vector
,
self
.
kmin
,
self
.
kmax
,
self
.
beta
)
a_des
=
-
self
.
kp
*
p_error
.
vector
-
self
.
k
v
*
v
_error
.
vector
+
self
.
_gMl
.
actInv
(
a_ref
).
vector
#sign +-
#
if self.expDecay is True:
#
self.kv = exponentialDecay(self.kp)
#
if self.adaptGain is True:
#
self.kp = adaptativeGain(p_error.vector, self.kmin, self.kmax, self.beta)
a_des
=
self
.
_gMl
.
actInv
(
a_ref
)
.
vector
-
(
self
.
k
p
*
self
.
p
_error
.
vector
+
self
.
kv
*
self
.
v_error
.
vector
)
J
=
self
.
robot
.
frameJacobian
(
q
,
self
.
_frame_id
,
False
)
if
(
local_frame
==
False
):
drift
=
self
.
_gMl
.
act
(
drift
);
a_des
[:
3
]
=
self
.
_gMl
.
rotation
*
a_des
[:
3
];
...
...
@@ -158,10 +163,7 @@ class SE3Task(Task):
J
[:
3
,:]
=
self
.
_gMl
.
rotation
*
J
[:
3
,:];
J
[
3
:,:]
=
self
.
_gMl
.
rotation
*
J
[
3
:,:];
self
.
p_error
=
p_error
.
vector
self
.
v_error
=
p_error
.
vector
return
J
[
self
.
_mask
,:],
drift
.
vector
[
self
.
_mask
],
a_des
[
self
.
_mask
]
return
J
[
self
.
_mask
,:]
*
self
.
__gain_matrix
,
drift
.
vector
[
self
.
_mask
],
a_des
[
self
.
_mask
]
def
jacobian
(
self
,
q
,
update_geometry
=
False
):
...
...
@@ -178,6 +180,7 @@ class CoMTask(Task):
self
.
_ref_trajectory
=
ref_trajectory
# mask over the desired euclidian axis
self
.
_mask
=
(
np
.
ones
(
3
)).
astype
(
bool
)
self
.
__gain_matrix
=
np
.
matrix
(
np
.
eye
(
robot
.
nv
))
@
property
def
dim
(
self
):
...
...
@@ -190,6 +193,10 @@ class CoMTask(Task):
assert
len
(
mask
)
==
3
,
"The mask must have 3 elements"
self
.
_mask
=
mask
.
astype
(
bool
)
def
setGain
(
self
,
gain_vector
):
assert
gain_vector
.
shape
==
(
self
.
robot
.
nv
,)
self
.
__gain_matrix
=
np
.
matrix
(
np
.
diag
(
gain_vector
))
def
dyn_value
(
self
,
t
,
q
,
v
,
update_geometry
=
False
):
# Get the current CoM position, velocity and acceleration
p_com
,
v_com
,
a_com
=
self
.
robot
.
com
(
q
,
v
,
0
*
v
)
...
...
@@ -208,7 +215,7 @@ class CoMTask(Task):
a_des
=
-
(
self
.
kp
*
self
.
p_error
+
self
.
kv
*
self
.
v_error
)
+
a_ref
# Compute jacobian
J
=
self
.
robot
.
Jcom
(
q
)
J
=
self
.
robot
.
Jcom
(
q
)
*
self
.
__gain_matrix
return
J
[
self
.
_mask
,:],
drift
[
self
.
_mask
],
a_des
[
self
.
_mask
]
...
...
@@ -380,38 +387,98 @@ class MomentumTask(Task):
self
.
__gain_matrix
=
np
.
matrix
(
np
.
diag
(
gain_vector
))
#self.__gain_matrix = np.matrix(np.matlib.repmat(gain,1,42))
def
dyn_value
(
self
,
t
,
q
,
v
):
(
hg_ref
,
vhg_ref
,
ahg_ref
)
=
self
.
_ref_traj
(
t
)
def
_getBiais
(
self
,
q
,
v
):
model
=
self
.
robot
.
model
data
=
self
.
robot
.
data
JMom
=
se3
.
ccrba
(
model
,
data
,
q
,
v
)
hg_act
=
self
.
robot
.
data
.
hg
.
np
.
A
.
copy
()
self
.
p_error
=
hg_act
[
self
.
_mask
,:]
-
hg_ref
[
self
.
_mask
,:]
self
.
v_error
=
self
.
robot
.
fext
[
self
.
_mask
,:]
-
vhg_ref
[
self
.
_mask
,:]
#***********************
p_com
=
data
.
com
[
0
]
cXi
=
SE3
.
Identity
()
oXi
=
self
.
robot
.
data
.
oMi
[
1
]
data
=
self
.
robot
.
data
p_com
=
self
.
robot
.
com
(
q
)
oXi
=
data
.
oMi
[
1
]
cXi
=
se3
.
SE3
.
Identity
()
cXi
.
rotation
=
oXi
.
rotation
cXi
.
translation
=
oXi
.
translation
-
p_com
#cXi.translation = p_com
m_gravity
=
model
.
gravity
.
copy
()
model
.
gravity
.
setZero
()
b
=
se3
.
nle
(
model
,
data
,
q
,
v
)
model
.
gravity
=
m_gravity
f_ff
=
se3
.
Force
(
b
[:
6
])
f_com
=
cXi
.
act
(
f_ff
)
hg_drift
=
f_com
.
angular
self
.
drift
=
f_com
.
np
[
self
.
_mask
,:]
#drift = np.matrix(np.zeros((3, 1)))
#a_tot = Ldot_des - hg_drift
#************************
self
.
a_des
=
-
(
self
.
kp
*
self
.
p_error
)
#self.a_des = -(self.kp * self.p_error) + (self.kv*v_error) + ahg_ref
#self.drift = 0*self.a_des
#print JMom.copy()[self._mask,:].shape
#print self.__gain_matrix.shape
return
f_com
.
np
def
_getContribution
(
self
,
com
,
s
):
'''
Get segment contribution to centroidal angular
momenta and its rate of change
Inputs:
- s : segment index
- com : position of the CoM in world reference frame
'''
# get spatial inertia and velocity
Y
=
self
.
robot
.
model
.
inertias
[
s
]
V
=
self
.
robot
.
data
.
v
[
s
]
# centroidal momenta in body frame
# ihi = I Vi
iHi
=
se3
.
Force
(
Y
.
matrix
()
*
V
.
vector
)
# rate of change of centroidal momenta in body frame
# ih_doti = Iai + Vi x* hi
# TO VERIFY
iHDi
=
(
self
.
robot
.
model
.
inertias
[
s
]
*
self
.
robot
.
data
.
a
[
s
])
+
se3
.
Inertia
.
vxiv
(
Y
,
V
)
#iHDi.linear
# express at the center of mass
oMc
=
se3
.
SE3
.
Identity
()
oMc
.
translation
=
self
.
robot
.
data
.
oMi
[
1
].
translation
-
com
# uncomment in case need to change the rotation of the reference frame
oMc
.
rotation
=
self
.
robot
.
data
.
oMi
[
1
].
rotation
cMi
=
oMc
.
actInv
(
self
.
robot
.
data
.
oMi
[
s
]
)
cHi
=
cMi
.
act
(
iHi
).
np
.
A1
cHDi
=
cMi
.
act
(
iHDi
).
np
.
A1
return
cHi
,
cHDi
def
_calculateHdot
(
self
,
q
):
# Individual contributions to centroidal momenta (and its rate of change)
segH
=
[];
segF
=
[]
p_com
=
self
.
robot
.
com
(
q
)
for
s
in
range
(
1
,
26
):
hc
,
hcd
=
self
.
_getContribution
(
p_com
,
s
)
segH
.
append
(
hc
)
segF
.
append
(
hcd
)
return
np
.
matrix
(
np
.
sum
(
np
.
array
(
segF
).
squeeze
(),
0
)).
T
def
dyn_value_h
(
self
,
t
,
q
,
v
):
(
self
.
hg_ref
,
self
.
dhg_ref
,
aahg_ref
)
=
self
.
_ref_traj
(
t
)
JMom
=
se3
.
ccrba
(
self
.
robot
.
model
,
self
.
robot
.
data
,
q
,
v
)
b
=
self
.
_getBiais
(
q
,
v
)
self
.
hg_act
=
self
.
robot
.
data
.
hg
.
np
.
A
.
copy
()
self
.
dhg_act
=
self
.
_calculateHdot
(
q
)
self
.
drift
=
b
[
self
.
_mask
,:]
self
.
p_error
=
self
.
hg_act
[
self
.
_mask
,:]
-
self
.
hg_ref
[
self
.
_mask
,:]
self
.
v_error
=
self
.
dhg_act
[
self
.
_mask
,:]
-
self
.
dhg_ref
[
self
.
_mask
,:]
#self.dh_des = -( (self.kp * self.p_error) + (self.kv*self.v_error) )
self
.
dh_des
=
-
(
self
.
kp
*
self
.
p_error
)
self
.
_jacobian
=
JMom
.
copy
()[
self
.
_mask
,:]
*
self
.
__gain_matrix
return
self
.
_jacobian
,
self
.
drift
,
self
.
a_des
#self.drift = 0*self.dh_des
return
self
.
_jacobian
,
self
.
drift
,
self
.
dh_des
def
dyn_value
(
self
,
t
,
q
,
v
):
(
self
.
hg_ref
,
self
.
dhg_ref
,
aahg_ref
)
=
self
.
_ref_traj
(
t
)
JMom
=
se3
.
ccrba
(
self
.
robot
.
model
,
self
.
robot
.
data
,
q
,
v
)
b
=
self
.
_getBiais
(
q
,
v
)
self
.
hg_act
=
self
.
robot
.
data
.
hg
.
np
.
A
.
copy
()
self
.
dhg_act
=
self
.
_calculateHdot
(
q
)
self
.
drift
=
b
[
self
.
_mask
,:]
self
.
p_error
=
self
.
hg_act
[
self
.
_mask
,:]
-
self
.
hg_ref
[
self
.
_mask
,:]
self
.
v_error
=
self
.
dhg_act
[
self
.
_mask
,:]
-
self
.
dhg_ref
[
self
.
_mask
,:]
self
.
dh_des
=
-
((
self
.
kp
*
self
.
p_error
)
+
(
self
.
kv
*
self
.
v_error
)
)
self
.
h_des
=
-
(
self
.
kp
*
self
.
p_error
)
self
.
_jacobian
=
JMom
.
copy
()[
self
.
_mask
,:]
*
self
.
__gain_matrix
#self.drift = 0*self.dh_des
return
self
.
_jacobian
,
self
.
drift
,
self
.
h_des
''' Define Fly Momentum Task '''
...
...
@@ -454,6 +521,7 @@ class FlyMomentumTask(Task):
#self.v_error = self.robot.fext[self._mask,:]
#self.v_error = data.hg.vector.copy()[self._mask,:] - vhg_ref[self._mask,:]
#self.a_des = -self.kv*self.v_error #+model.gravity.vector[self._mask,:]
self
.
a_des
=
self
.
kv
*
self
.
robot
.
fext
[
self
.
_mask
,:]
#vhg_ref #+model.gravity.vector[self._mask,:]
#self.drift = 0 * self.a_des
#self.a_des =
...
...
hqp/trajectories.py
View file @
d17547ab
...
...
@@ -44,7 +44,7 @@ class VaryingNdTrajectory (object):
assert
t
>=
0.0
,
"Time must be non-negative"
i
=
int
(
t
/
self
.
_dt
);
if
(
i
>=
self
.
_x_ref
.
shape
[
1
]):
raise
ValueError
(
"Specified time exceeds the duration of the trajectory: "
+
str
(
t
))
raise
ValueError
(
"Specified time exceeds the duration of the trajectory: "
+
str
(
t
))
return
(
self
.
_x_ref
[:,
i
],
self
.
_v_ref
[:,
i
],
self
.
_a_ref
[:,
i
]);
...
...
@@ -159,3 +159,27 @@ class SmoothedSE3Trajectory (object):
v
.
linear
=
self
.
_v_ref
[:,
i
];
a
.
linear
=
self
.
_a_ref
[:,
i
];
return
(
M
,
v
,
a
);
''' An Nd trajectory with constant state and velocity and zero acceleration. '''
class
ConstantNdTrajectoryXV
(
object
):
def
__init__
(
self
,
name
,
x_ref
,
v_ref
):
self
.
_name
=
name
self
.
_dim
=
x_ref
.
shape
[
0
]
self
.
_x_ref
=
np
.
matrix
.
copy
(
x_ref
);
self
.
_v_ref
=
np
.
matrix
.
copy
(
v_ref
);
self
.
_a_ref
=
np
.
zeros
(
x_ref
.
shape
);
@
property
def
dim
(
self
):
return
self
.
_dim
def
setReference
(
self
,
x_ref
):
assert
x_ref
.
shape
[
0
]
==
self
.
_x_ref
.
shape
[
0
]
self
.
_x_ref
=
x_ref
;
self
.
_v_ref
=
v_ref
;
def
__call__
(
self
,
t
):
return
(
self
.
_x_ref
,
self
.
_v_ref
,
self
.
_a_ref
);
hqp/wrapper.py
View file @
d17547ab
...
...
@@ -84,9 +84,7 @@ class Wrapper():
se3
.
computeJacobians
(
self
.
model
,
self
.
data
,
q
)
se3
.
rnea
(
self
.
model
,
self
.
data
,
q
,
self
.
v
,
self
.
a
)
self
.
biais
(
self
.
q
,
self
.
v
)
self
.
q
=
q
.
copy
()
self
.
q
=
q
.
copy
()
def
parseTrial
(
self
,
data
):
q
=
[]
...
...
@@ -151,6 +149,11 @@ class Wrapper():
''' the coriolis, centrifugal and gravitational effects '''
return
se3
.
nle
(
self
.
model
,
self
.
data
,
q
,
v
)
def
bias
(
self
,
q
,
v
,
update_kinematics
=
True
):
if
(
update_kinematics
):
return
se3
.
nle
(
self
.
model
,
self
.
data
,
q
,
v
)
return
self
.
data
.
nle
def
generalizedAcceleration
(
self
,
V
,
dt
):
return
np
.
asmatrix
(
np
.
gradient
(
V
,
dt
))
...
...
@@ -277,15 +280,23 @@ class Wrapper():
def
com
(
self
,
q
,
v
=
None
,
a
=
None
,
update_kinematics
=
True
):
if
v
is
not
None
:
if
a
is
None
:
se3
.
centerOfMass
(
self
.
model
,
self
.
data
,
q
,
v
,
update_kinematics
)
se3
.
centerOfMass
(
self
.
model
,
self
.
data
,
q
,
v
)
#
, update_kinematics)
return
self
.
data
.
com
[
0
],
self
.
data
.
vcom
[
0
]
se3
.
centerOfMass
(
self
.
model
,
self
.
data
,
q
,
v
,
a
,
update_kinematics
)
se3
.
centerOfMass
(
self
.
model
,
self
.
data
,
q
,
v
,
a
)
#
, update_kinematics)
return
self
.
data
.
com
[
0
],
self
.
data
.
vcom
[
0
],
self
.
data
.
acom
[
0
]
return
se3
.
centerOfMass
(
self
.
model
,
self
.
data
,
q
,
update_kinematics
)
return
se3
.
centerOfMass
(
self
.
model
,
self
.
data
,
q
)
#
, update_kinematics)
def
Jcom
(
self
,
q
):
def
Jcom
(
self
,
q
):
#, update_kinematics=True):
return
se3
.
jacobianCenterOfMass
(
self
.
model
,
self
.
data
,
q
)
#if(update_kinematics):
# return se3.jacobianCenterOfMass(self.model, self.data, q)
#return self.data.Jcom
def
mass
(
self
,
q
,
update_kinematics
=
True
):
if
(
update_kinematics
):
return
se3
.
crba
(
self
.
model
,
self
.
data
,
q
)
return
self
.
data
.
M
def
getDoF
(
self
,
jointName
):
idx
=
self
.
model
.
getJointId
(
jointName
)
if
idx
<
len
(
self
.
model
.
joints
):
...
...
@@ -304,6 +315,12 @@ class Wrapper():
def
computeJacobians
(
self
,
q
):
return
se3
.
computeJacobians
(
self
.
model
,
self
.
data
,
q
)
''' Compute the placements of all the operational frames and put the results in data.
To be called after forwardKinematics.
'''
def
framesKinematics
(
self
,
q
):
se3
.
framesKinematics
(
self
.
model
,
self
.
data
,
q
)
def
framePosition
(
self
,
index
,
q
=
None
):
f
=
self
.
model
.
frames
[
index
]
if
q
is
not
None
:
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment