Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Casadi Dimension mismatch #11

Open
hardyPyz opened this issue Jun 5, 2024 · 0 comments
Open

Casadi Dimension mismatch #11

hardyPyz opened this issue Jun 5, 2024 · 0 comments

Comments

@hardyPyz
Copy link

hardyPyz commented Jun 5, 2024

Below is a code that reproduces the issue:
import pinocchio as pin
import numpy as np
import casadi as ca
import sys
import os
from os.path import dirname, join, abspath
import pinocchio.casadi as cpin
from pinocchio.visualize import GepettoVisualizer

pinocchio_model_dir = join(dirname(str(abspath(file))), "URDF/a1")
urdf_filename = pinocchio_model_dir + '/a1.urdf'
mesh_dir = join(pinocchio_model_dir,"meshes")

model, collision_model, visual_model = pin.buildModelsFromUrdf(urdf_filename, mesh_dir)
data = model.createData()

q_init = pin.neutral(model)

hip = np.pi/4
thigh = -np.pi/2
calf = np.pi/2
hight = -0.1
q_target = np.array([0., 0., 0., 0., 0., hight, 1.,
hip, thigh, calf,
hip, thigh, calf,
hip, thigh, calf,
hip, thigh, calf])

cmodel = cpin.Model(model)
cdata = cmodel.createData()
nq = cmodel.nq # 19
nv = cmodel.nv # 18
nx = nq + nv
x = ca.SX.sym("x",nx,1)
q = x[:nq]
v = x[nq:]
N = 100

foot1 = cmodel.getFrameId("FR_foot")
foot2 = cmodel.getFrameId("FL_foot")
foot3 = cmodel.getFrameId("RR_foot")
foot4 = cmodel.getFrameId("RL_foot")
J1_SX = cpin.computeFrameJacobian(cmodel, cdata, q, foot1, cpin.ReferenceFrame.WORLD)
J1_SX = cpin.computeFrameJacobian(cmodel, cdata, q, foot1, cpin.ReferenceFrame.WORLD)
J2_SX = cpin.computeFrameJacobian(cmodel, cdata, q, foot2, cpin.ReferenceFrame.WORLD)
J3_SX = cpin.computeFrameJacobian(cmodel, cdata, q, foot3, cpin.ReferenceFrame.WORLD)
J4_SX = cpin.computeFrameJacobian(cmodel, cdata, q, foot4, cpin.ReferenceFrame.WORLD)
J1 = ca.Function("JFR_foot",[q],[J1_SX])
J2 = ca.Function("JFL_foot",[q],[J2_SX])
J3 = ca.Function("JRR_foot",[q],[J3_SX])
J4 = ca.Function("JRL_foot",[q],[J4_SX])
tau = ca.SX.sym('tau',nv,1)
a_fun = ca.Function("aba",[q,v,tau],[cpin.aba(cmodel,cdata,q,v,tau)])
M_fun = ca.Function("crba",[q],[cpin.crba(cmodel,cdata,q)])
H_fun = ca.Function("nonLinearEffects",[q,v],[cpin.nonLinearEffects(cmodel,cdata,q,v)])

J_fun = ca.Function("Jacobians",[q],[cpin.computeJointJacobians(cmodel,cdata,q)])

opti = ca.Opti()
q_vars = opti.variable(nq, N+1)
v_vars = opti.variable(nv, N+1)
a_vars = opti.variable(nv, N)
tau_vars = opti.variable(nv, N)
f_vars = opti.variable(24,N)

for k in range(N):
qk = q_vars[:, k]
vk = v_vars[:, k]
tauk= tau_vars[:, k]
fk = f_vars[:,k]

J1_MX = J1(qk)
J2_MX = J2(qk)
J3_MX = J3(qk)
J4_MX = J4(qk)
JT = ca.transpose(ca.vertcat(J1_MX,J2_MX,J3_MX,J4_MX))

dyn = ca.reshape(tauk + JT* fk - M_fun(qk) * a_fun(qk,vk,tauk) - H_fun(qk,vk) , (-1,1))

opti.subject_to( dyn <= 1e-2 )
opti.subject_to( dyn >= -1e-2 )

opti.subject_to(q_vars[:,0] == q_init)
opti.subject_to(q_vars[:,-1] == q_target)

obj = 0
opti.minimize(obj)

opts = {'ipopt.print_level': 0, 'print_time': 0}
opti.solver("ipopt",opts)

sol = opti.solve()

optimal_solution = sol.value(q_vars)
print("result",optimal_solution)

The error message says:
.../casadi/core/mx.cpp:527: Dimension mismatch for (xy), x is 18x24, while y is 24x1
File "/home/rl/Desktop/test2.py", line 85, in
dyn = ca.reshape(tauk + JT
fk - M_fun(qk) * a_fun(qk,vk,tauk) - H_fun(qk,vk) , (-1,1))
RuntimeError: .../casadi/core/mx.cpp:527: Dimension mismatch for (x*y), x is 18x24, while y is 24x1

Why JT and fk don't match?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant