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
Stack Of Tasks
sot-torque-control
Commits
9a95781d
Commit
9a95781d
authored
Dec 12, 2017
by
andreadelprete
Browse files
[adm-ctrl] Fix bug in assert
parent
4e566d56
Changes
2
Hide whitespace changes
Inline
Side-by-side
scripts/python_commands.txt
View file @
9a95781d
...
...
@@ -18,10 +18,10 @@ create_topic(robot.ros, robot.traj_gen.q, 'q_ref');
create_topic(robot.ros, robot.device.robotVelocity, 'v');
create_topic(robot.ros, robot.base_estimator.v, 'v_est', robot, robot.base_estimator);
create_topic(robot.ros, robot.inv_dyn.zmp_des, 'zmp_des');
create_topic(robot.ros, robot.inv_dyn.zmp_ref, 'zmp_ref');
#
create_topic(robot.ros, robot.inv_dyn.zmp_ref, 'zmp_ref');
create_topic(robot.ros, robot.inv_dyn.zmp, 'zmp');
create_topic(robot.ros, robot.inv_dyn.f_ref_right_foot, 'f_ref_right_foot');
create_topic(robot.ros, robot.inv_dyn.f_ref_left_foot, 'f_ref_left_foot');
#
create_topic(robot.ros, robot.inv_dyn.f_ref_right_foot, 'f_ref_right_foot');
#
create_topic(robot.ros, robot.inv_dyn.f_ref_left_foot, 'f_ref_left_foot');
create_topic(robot.ros, robot.inv_dyn.f_des_right_foot, 'f_des_right_foot');
create_topic(robot.ros, robot.inv_dyn.f_des_left_foot, 'f_des_left_foot');
create_topic(robot.ros, robot.inv_dyn.zmp_left_foot, 'zmp_left_foot');
...
...
@@ -49,7 +49,8 @@ create_topic(robot.ros, robot.inv_dyn.dq_admittance, 'dq_adm');
#robot.torque_ctrl.velLeakingRate.value = 30*(0.,)
#robot.torque_ctrl.reset_ddq_integral();
ent.ctrl_manager.setCtrlMode('all','torque')
#ent.ctrl_manager.setCtrlMode('all','torque')
robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lar-lap','torque')
robot.inv_dyn.dq_admittance.recompute(0)
robot.base_estimator.set_imu_weight(0.0);
plug(robot.encoders.sout, robot.base_estimator.joint_positions)
...
...
src/admittance-controller.cpp
View file @
9a95781d
...
...
@@ -235,9 +235,9 @@ namespace dynamicgraph
//const Vector& qMask = m_controlledJointsSIN(iter); // n
//const Eigen::Vector4d& damping = m_dampingSIN(iter); // 4
assert
(
q
.
size
()
==
m_nj
&&
"Unexpected size of signal encoder"
);
assert
(
dq
.
size
()
==
m_nj
&&
"Unexpected size of signal dq"
);
assert
(
qMask
.
size
()
==
m_nj
&&
"Unexpected size of signal controlledJoints"
);
assert
(
q
_sot
.
size
()
==
m_nj
&&
"Unexpected size of signal encoder"
);
//
assert(dq.size()==m_nj && "Unexpected size of signal dq");
//
assert(qMask.size()==m_nj && "Unexpected size of signal controlledJoints");
Eigen
::
Vector6d
v_des_RF
=
-
kp
.
cwiseProduct
(
e_f_RF
);
Eigen
::
Vector6d
v_des_LF
=
-
kp
.
cwiseProduct
(
e_f_LF
);
...
...
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