Skip to content
Snippets Groups Projects
Commit cc1d33e0 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

np.matrix → np.array

parent 0928808f
No related branches found
No related tags found
No related merge requests found
Pipeline #28061 failed
Stage: test
Hide whitespace changes
Inline Side-by-side
tests/python/test_ContactPoint.py
+ 4
− 4
View file @ cc1d33e0
......@@ -25,7 +25,7 @@ mu = 0.3
fMin = 10.0
fMax = 1000.0
frameName = "RAnkleRoll"
contactNormal = np.matrix(np.zeros(3)).transpose()
contactNormal = np.zeros(3)
contactNormal[2] = 1.0
contact = tsid.ContactPoint("contactPoint", robot, frameName, contactNormal, mu, fMin, fMax)
......@@ -33,7 +33,7 @@ contact = tsid.ContactPoint("contactPoint", robot, frameName, contactNormal, mu,
assert contact.n_motion == 3
assert contact.n_force == 3
Kp = np.matrix(np.ones(3)).transpose()
Kp = np.ones(3)
Kd = 2 * Kp
contact.setKp(Kp)
contact.setKd(Kd)
......@@ -42,7 +42,7 @@ assert np.linalg.norm(contact.Kp - Kp, 2) < tol
assert np.linalg.norm(contact.Kd - Kd, 2) < tol
q = model.neutralConfiguration
v = np.matrix(np.zeros(robot.nv)).transpose()
v = np.zeros(robot.nv)
robot.computeAllTerms(data, q, v)
H_ref = robot.position(data, robot.model().getJointId(frameName))
......@@ -51,7 +51,7 @@ contact.setReference(H_ref)
t = 0.0
contact.computeMotionTask(t, q, v, data)
forceIneq = contact.computeForceTask(t, q, v, data)
f = np.matrix(np.zeros(3)).transpose()
f = np.zeros(3)
f[2] = 100.0
assert (forceIneq.matrix * f <= forceIneq.upperBound).all()
......
tests/python/test_Formulation.py
+ 42
− 27
View file @ cc1d33e0
import copy
import os
import numpy as np
import pinocchio as se3
from numpy.linalg import norm
import tsid
from numpy.linalg import norm
print("")
print("Test InvDyn")
print("")
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../../models/romeo'
urdf = path + '/urdf/romeo.urdf'
path = filename + "/../../models/romeo"
urdf = path + "/urdf/romeo.urdf"
vector = se3.StdVec_StdString()
vector.extend(item for item in path)
robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
srdf = path + '/srdf/romeo_collision.srdf'
srdf = path + "/srdf/romeo_collision.srdf"
model = robot.model()
se3.loadReferenceConfigurations(model, srdf, False)
q = model.referenceConfigurations["half_sitting"]
q[2] += 0.84
v = np.matrix(np.zeros(robot.nv)).transpose()
v = np.zeros(robot.nv)
t = 0.0
lxp = 0.14
......@@ -39,7 +36,7 @@ fMin = 5.0
fMax = 1000.0
rf_frame_name = "RAnkleRoll"
lf_frame_name = "LAnkleRoll"
contactNormal = np.matrix([0.0, 0.0, 1.0]).transpose()
contactNormal = np.array([0.0, 0.0, 1.0])
w_com = 1.0
w_posture = 1e-2
w_forceRef = 1e-5
......@@ -53,34 +50,52 @@ assert robot.model().existFrame(lf_frame_name)
invdyn = tsid.InverseDynamicsFormulationAccForce("tsid", robot, False)
invdyn.computeProblemData(t, q, v)
data = invdyn.data()
contact_Point = np.matrix(np.ones((3, 4)) * lz)
contact_Point = np.ones((3, 4)) * lz
contact_Point[0, :] = [-lxn, -lxn, lxp, lxp]
contact_Point[1, :] = [-lyn, lyp, -lyn, lyp]
contactRF = tsid.Contact6d("contact_rfoot", robot, rf_frame_name, contact_Point, contactNormal, mu, fMin, fMax,
w_forceRef)
contactRF.setKp(kp_contact * np.matrix(np.ones(6)).transpose())
contactRF.setKd(2.0 * np.sqrt(kp_contact) * np.matrix(np.ones(6)).transpose())
contactRF = tsid.Contact6d(
"contact_rfoot",
robot,
rf_frame_name,
contact_Point,
contactNormal,
mu,
fMin,
fMax,
w_forceRef,
)
contactRF.setKp(kp_contact * np.ones(6))
contactRF.setKd(2.0 * np.sqrt(kp_contact) * np.ones(6))
H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))
contactRF.setReference(H_rf_ref)
invdyn.addRigidContact(contactRF)
contactLF = tsid.Contact6d("contact_lfoot", robot, lf_frame_name, contact_Point, contactNormal, mu, fMin, fMax,
w_forceRef)
contactLF.setKp(kp_contact * np.matrix(np.ones(6)).transpose())
contactLF.setKd(2.0 * np.sqrt(kp_contact) * np.matrix(np.ones(6)).transpose())
contactLF = tsid.Contact6d(
"contact_lfoot",
robot,
lf_frame_name,
contact_Point,
contactNormal,
mu,
fMin,
fMax,
w_forceRef,
)
contactLF.setKp(kp_contact * np.ones(6))
contactLF.setKd(2.0 * np.sqrt(kp_contact) * np.ones(6))
H_lf_ref = robot.position(data, robot.model().getJointId(lf_frame_name))
contactLF.setReference(H_lf_ref)
invdyn.addRigidContact(contactLF)
comTask = tsid.TaskComEquality("task-com", robot)
comTask.setKp(kp_com * np.matrix(np.ones(3)).transpose())
comTask.setKd(2.0 * np.sqrt(kp_com) * np.matrix(np.ones(3)).transpose())
comTask.setKp(kp_com * np.ones(3))
comTask.setKd(2.0 * np.sqrt(kp_com) * np.ones(3))
invdyn.addMotionTask(comTask, w_com, 1, 0.0)
postureTask = tsid.TaskJointPosture("task-posture", robot)
postureTask.setKp(kp_posture * np.matrix(np.ones(robot.nv - 6)).transpose())
postureTask.setKd(2.0 * np.sqrt(kp_posture) * np.matrix(np.ones(robot.nv - 6)).transpose())
postureTask.setKp(kp_posture * np.ones(robot.nv - 6))
postureTask.setKd(2.0 * np.sqrt(kp_posture) * np.ones(robot.nv - 6))
invdyn.addMotionTask(postureTask, w_posture, 1, 0.0)
########### Test 1 ##################3
......@@ -92,16 +107,16 @@ kp_RF = 100.0
w_RF = 1e3
max_it = 1000
rightFootTask = tsid.TaskSE3Equality("task-right-foot", robot, rf_frame_name)
rightFootTask.setKp(kp_RF * np.matrix(np.ones(6)).transpose())
rightFootTask.setKd(2.0 * np.sqrt(kp_com) * np.matrix(np.ones(6)).transpose())
rightFootTask.setKp(kp_RF * np.ones(6))
rightFootTask.setKd(2.0 * np.sqrt(kp_com) * np.ones(6))
H_rf_ref = robot.position(data, robot.model().getJointId(rf_frame_name))
invdyn.addMotionTask(rightFootTask, w_RF, 1, 0.0)
s = tsid.TrajectorySample(12, 6)
H_rf_ref_vec = np.matrix(np.zeros(12)).transpose()
H_rf_ref_vec = np.zeros(12)
H_rf_ref_vec[0:3] = H_rf_ref.translation
for i in range(0, 3):
H_rf_ref_vec[3 * i + 3:3 * i + 6] = H_rf_ref.rotation[:, i]
H_rf_ref_vec[3 * i + 3 : 3 * i + 6] = H_rf_ref.rotation[:, i]
s.value(H_rf_ref_vec)
rightFootTask.setReference(s)
......@@ -117,7 +132,7 @@ samplePosture = tsid.TrajectorySample(robot.nv - 6)
solver = tsid.SolverHQuadProg("qp solver")
solver.resize(invdyn.nVar, invdyn.nEq, invdyn.nIn)
tau_old = np.matrix(np.zeros(robot.nv - 6)).transpose()
tau_old = np.zeros(robot.nv - 6)
for i in range(0, max_it):
if i == REMOVE_CONTACT_N:
......
tests/python/test_RobotWrapper.py
+ 7
− 8
View file @ cc1d33e0
......@@ -2,7 +2,6 @@ import os
import numpy as np
import pinocchio as se3
import tsid
print("")
......@@ -11,26 +10,26 @@ print("")
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../../models/romeo'
urdf = path + '/urdf/romeo.urdf'
path = filename + "/../../models/romeo"
urdf = path + "/urdf/romeo.urdf"
vector = se3.StdVec_StdString()
vector.extend(item for item in path)
robot = tsid.RobotWrapper(urdf, vector, se3.JointModelFreeFlyer(), False)
model = robot.model()
lb = model.lowerPositionLimit
lb[0:3] = -10.0 * np.matrix(np.ones(3)).transpose()
lb[3:7] = -1.0 * np.matrix(np.ones(4)).transpose()
lb[0:3] = -10.0 * np.ones(3)
lb[3:7] = -1.0 * np.ones(4)
ub = model.upperPositionLimit
ub[0:3] = 10.0 * np.matrix(np.ones(3)).transpose()
ub[3:7] = 1.0 * np.matrix(np.ones(4)).transpose()
ub[0:3] = 10.0 * np.ones(3)
ub[3:7] = 1.0 * np.ones(4)
q = se3.randomConfiguration(robot.model(), lb, ub)
print(q.transpose())
data = robot.data()
v = np.matrix(np.ones(robot.nv)).transpose()
v = np.ones(robot.nv)
robot.computeAllTerms(data, q, v)
print(robot.com(data))
......
tests/python/test_Tasks.py
+ 61
− 34
View file @ cc1d33e0
import copy
import os
import numpy as np
import pinocchio as pin
from numpy.linalg import norm
import tsid
from numpy.linalg import norm
print("")
print("Test Task COM")
......@@ -14,15 +12,15 @@ print("")
tol = 1e-5
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../../models/romeo'
urdf = path + '/urdf/romeo.urdf'
path = filename + "/../../models/romeo"
urdf = path + "/urdf/romeo.urdf"
vector = pin.StdVec_StdString()
vector.extend(item for item in path)
robot = tsid.RobotWrapper(urdf, vector, pin.JointModelFreeFlyer(), False)
model = robot.model()
data = robot.data()
srdf = path + '/srdf/romeo_collision.srdf'
srdf = path + "/srdf/romeo_collision.srdf"
pin.loadReferenceConfigurations(model, srdf, False)
q = model.referenceConfigurations["half_sitting"]
......@@ -33,24 +31,24 @@ pin.centerOfMass(model, data, q)
taskCOM = tsid.TaskComEquality("task-com", robot)
Kp = 100 * np.matrix(np.ones(3)).transpose()
Kd = 20.0 * np.matrix(np.ones(3)).transpose()
Kp = 100 * np.ones(3)
Kd = 20.0 * np.ones(3)
taskCOM.setKp(Kp)
taskCOM.setKd(Kd)
assert np.linalg.norm(Kp - taskCOM.Kp, 2) < tol
assert np.linalg.norm(Kd - taskCOM.Kd, 2) < tol
com_ref = data.com[0] + np.matrix(np.ones(3) * 0.02).transpose()
com_ref = data.com[0] + np.ones(3) * 0.02
traj = tsid.TrajectoryEuclidianConstant("traj_se3", com_ref)
sample = tsid.TrajectorySample(0)
t = 0.0
dt = 0.001
max_it = 1000
Jpinv = np.matrix(np.zeros((robot.nv, 3)))
Jpinv = np.zeros((robot.nv, 3))
error_past = 1e100
v = np.matrix(np.zeros(robot.nv)).transpose()
v = np.zeros(robot.nv)
for i in range(0, max_it):
robot.computeAllTerms(data, q, v)
......@@ -73,7 +71,14 @@ for i in range(0, max_it):
print("Success Convergence")
break
if i % 100 == 0:
print("Time :", t, "COM pos error :", error, "COM vel error :", np.linalg.norm(taskCOM.velocity_error, 2))
print(
"Time :",
t,
"COM pos error :",
error,
"COM vel error :",
np.linalg.norm(taskCOM.velocity_error, 2),
)
print("")
print("Test Task Joint Posture")
......@@ -85,15 +90,15 @@ q[2] += 0.84
task_joint = tsid.TaskJointPosture("task-posture", robot)
na = robot.nv - 6
Kp = 100 * np.matrix(np.ones(na)).transpose()
Kd = 20.0 * np.matrix(np.ones(na)).transpose()
Kp = 100 * np.ones(na)
Kd = 20.0 * np.ones(na)
task_joint.setKp(Kp)
task_joint.setKd(Kd)
assert np.linalg.norm(Kp - task_joint.Kp, 2) < tol
assert np.linalg.norm(Kd - task_joint.Kd, 2) < tol
q_ref = np.matrix(np.random.randn(na)).transpose()
q_ref = np.random.randn(na)
traj = tsid.TrajectoryEuclidianConstant("traj_joint", q_ref)
sample = tsid.TrajectorySample(0)
......@@ -122,8 +127,14 @@ for i in range(0, max_it):
print("Success Convergence")
break
if i % 100 == 0:
print("Time :", t, "Joint pos error :", error, "Joint vel error :",
np.linalg.norm(task_joint.velocity_error, 2))
print(
"Time :",
t,
"Joint pos error :",
error,
"Joint vel error :",
np.linalg.norm(task_joint.velocity_error, 2),
)
print("")
print("Test Task SE3")
......@@ -135,8 +146,8 @@ q[2] += 0.84
task_se3 = tsid.TaskSE3Equality("task-se3", robot, "RWristPitch")
na = 6
Kp = 100 * np.matrix(np.ones(na)).transpose()
Kd = 20.0 * np.matrix(np.ones(na)).transpose()
Kp = 100 * np.ones(na)
Kd = 20.0 * np.ones(na)
task_se3.setKp(Kp)
task_se3.setKd(Kd)
......@@ -175,7 +186,14 @@ for i in range(0, max_it):
print("Success Convergence")
break
if i % 100 == 0:
print("Time :", t, "EE pos error :", error, "EE vel error :", np.linalg.norm(task_se3.velocity_error, 2))
print(
"Time :",
t,
"EE pos error :",
error,
"EE vel error :",
np.linalg.norm(task_se3.velocity_error, 2),
)
print("")
print("Test Task Angular Momentum")
......@@ -183,15 +201,15 @@ print("")
tol = 1e-5
filename = str(os.path.dirname(os.path.abspath(__file__)))
path = filename + '/../../models/romeo'
urdf = path + '/urdf/romeo.urdf'
path = filename + "/../../models/romeo"
urdf = path + "/urdf/romeo.urdf"
vector = pin.StdVec_StdString()
vector.extend(item for item in path)
robot = tsid.RobotWrapper(urdf, vector, pin.JointModelFreeFlyer(), False)
model = robot.model()
data = robot.data()
srdf = path + '/srdf/romeo_collision.srdf'
srdf = path + "/srdf/romeo_collision.srdf"
pin.loadReferenceConfigurations(model, srdf, False)
q = model.referenceConfigurations["half_sitting"]
......@@ -200,24 +218,24 @@ print("q:", q.transpose())
taskAM = tsid.TaskAMEquality("task-AM", robot)
Kp = 100 * np.matrix(np.ones(3)).transpose()
Kd = 20.0 * np.matrix(np.ones(3)).transpose()
Kp = 100 * np.ones(3)
Kd = 20.0 * np.ones(3)
taskAM.setKp(Kp)
taskAM.setKd(Kd)
assert np.linalg.norm(Kp - taskAM.Kp, 2) < tol
assert np.linalg.norm(Kd - taskAM.Kd, 2) < tol
am_ref = np.matrix(np.zeros(3)).transpose()
am_ref = np.zeros(3)
traj = tsid.TrajectoryEuclidianConstant("traj_se3", am_ref)
sample = tsid.TrajectorySample(0)
t = 0.0
dt = 0.001
max_it = 1000
Jpinv = np.matrix(np.zeros((robot.nv, 3)))
Jpinv = np.zeros((robot.nv, 3))
error_past = 1e100
v = np.matrix(np.random.randn(robot.nv)).transpose()
v = np.random.randn(robot.nv)
for i in range(0, max_it):
robot.computeAllTerms(data, q, v)
......@@ -264,19 +282,22 @@ print("")
from generator import create_7dof_arm
# Get robot model
model, geom_model = create_7dof_arm() # A robot containing sperical joints where nq != nv
(
model,
geom_model,
) = create_7dof_arm() # A robot containing sperical joints where nq != nv
# Initialize problem
robot = tsid.RobotWrapper(model, tsid.FIXED_BASE_SYSTEM, False)
data = robot.data()
q = pin.neutral(model)
v = np.matrix(np.zeros(robot.nv)).transpose()
v = np.zeros(robot.nv)
task_joint = tsid.TaskJointPosture("task-posture-uncommon", robot)
Kp = 100 * np.matrix(np.ones(robot.nv)).transpose()
Kd = 20.0 * np.matrix(np.ones(robot.na)).transpose()
Kp = 100 * np.ones(robot.nv)
Kd = 20.0 * np.ones(robot.na)
task_joint.setKp(Kp)
task_joint.setKd(Kd)
......@@ -314,7 +335,13 @@ for i in range(0, max_it):
print("Success Convergence")
break
if i % 100 == 0:
print("Time :", t, "Joint pos error :", error, "Joint vel error :",
np.linalg.norm(task_joint.velocity_error, 2))
print(
"Time :",
t,
"Joint pos error :",
error,
"Joint vel error :",
np.linalg.norm(task_joint.velocity_error, 2),
)
print("All test is done")
tests/python/test_Trajectories.py
+ 5
− 6
View file @ cc1d33e0
import numpy as np
import pinocchio as se3
import tsid
print("")
......@@ -10,8 +9,8 @@ print("")
tol = 1e-5
n = 5
q_ref = np.matrix(np.ones(n)).transpose()
zero = np.matrix(np.zeros(n)).transpose()
q_ref = np.ones(n)
zero = np.zeros(n)
traj_euclidian = tsid.TrajectoryEuclidianConstant("traj_eucl", q_ref)
assert traj_euclidian.has_trajectory_ended()
......@@ -29,12 +28,12 @@ print("Test Trajectory SE3")
print("")
M_ref = se3.SE3.Identity()
M_vec = np.matrix(np.zeros(12)).transpose()
M_vec = np.zeros(12)
M_vec[0:3] = M_ref.translation
zero = np.matrix(np.zeros(6)).transpose()
zero = np.zeros(6)
for i in range(1, 4):
M_vec[3 * i:3 * i + 3] = M_ref.rotation[:, i - 1]
M_vec[3 * i : 3 * i + 3] = M_ref.rotation[:, i - 1]
traj_se3 = tsid.TrajectorySE3Constant("traj_se3")
traj_se3.setReference(M_ref)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment