You signed in with another tab or window. Reload to refresh your session.You signed out in another tab or window. Reload to refresh your session.You switched accounts on another tab or window. Reload to refresh your session.Dismiss alert
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
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?
The text was updated successfully, but these errors were encountered:
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]
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?
The text was updated successfully, but these errors were encountered: