Open Timothee-ANNE opened 1 year ago
@Timothee-ANNE do you have a proposal for fixing this? Can you make a PR?
I don't remember exactly. I think I hacked it by changing by hand the gripper position sent to Dart:
`
auto q = controller->q(false);
timer.end("solver");
Eigen::VectorXd q_no_mimic = controller->filter_cmd(q).tail(ncontrollable); //no FB
Eigen::VectorXd q_damaged = inria_wbc::robot_dart::filter_cmd(q_no_mimic, controllable_dofs, active_dofs_controllable);
// Close Gripper
static float gripper_init = q_damaged(21);
if (behavior_name == "door_opening"){
// closing gripper
if (simu->scheduler().current_time() >= close_gripper_time && simu->scheduler().current_time() < close_gripper_time + close_gripper_duration) {
if (simu->scheduler().current_time() >= close_gripper_time and simu->scheduler().current_time() <= close_gripper_time +dt)
std::cout << "closing gripper" << std::endl;
float alpha = (simu->scheduler().current_time() - close_gripper_time) / close_gripper_duration;
q_damaged(29) = (1-alpha) * gripper_init + alpha * close_gripper_joint;
} // keep gripper closed
else if (simu->scheduler().current_time() >= close_gripper_time + close_gripper_duration && simu->scheduler().current_time() < reopen_gripper_time){
q_damaged(29) = close_gripper_joint;
} // reopen gripper
else if (simu->scheduler().current_time() >= reopen_gripper_time && simu->scheduler().current_time() < reopen_gripper_time + reopen_gripper_duration){
float alpha = (simu->scheduler().current_time() - reopen_gripper_time) / reopen_gripper_duration;
q_damaged(29) = (1-alpha) * close_gripper_joint + alpha * gripper_init;
} else {
q_damaged(29) = gripper_init;
}
}
timer.begin("cmd");
if (vm["actuators"].as<std::string>() == "velocity" || vm["actuators"].as<std::string>() == "servo")
cmd = inria_wbc::robot_dart::compute_velocities(robot, q_damaged, 1. / control_freq, active_dofs_controllable);
else if (vm["actuators"].as<std::string>() == "spd") {
cmd = inria_wbc::robot_dart::compute_spd(robot, q_damaged, 1. / control_freq, active_dofs_controllable, false);
}
else // torque
cmd = controller->tau(false);
timer.end("cmd");
`
With the current friction (1.0) in the Talos' gripper_right_joint and gripper_left_joint the spd controller cannot smoothly open or close the gripper.