Set correctly SERVO motors parameters

[pet93]

Hi everyone! I'm working at a Nao robot simulation that performs with VELOCITY actuator_type but I have to switch to SERVO mode. Once done, robot falls down due to gravity force, and, regardless of this, (once set gravity to zero) it doesn't move all joints as desired, some of these it doesn't move at all! I've put following commands: for (int i=0; i<numdof; ++i) naoRobot->getJoint(joints[i])->setForceUpperLimit(0, sufficient_force); naoRobot->getJoint(joints[i])->setForceLowerLimit(0, -sufficient_force); naoRobot->getJoint(joints[i])->setDampingCoefficient(0, 0.0); naoRobot->getJoint(joints[i])->setSpringStiffness(0, 0.0); naoRobot->getJoint(joints[i])->setCoulombFriction(0, 0.0); naoRobot->getJoint(joints[i])->setPositionLimitEnforced(true); naoRobot->getJoint(joints[i])->setPositionUpperLimit(0, posUpperLimit); naoRobot->getJoint(joints[i])->setPositionLowerLimit(0, posLowerLimit);

The problem due to gravity seems solved but robot still doesn't move some joint, even though I've considered ALL ones in these above commands. I have to set further parameters?

[jslee02]

Your code looks fine, but there could be various reason why your joints don't move as you expected: such as conflicting the constraints each other like collision and joint constraints. If it's okay, then it would be easier to look at your code and debug it. So please consider sharing your code or making minimal code that reproduces your issue.

[pet93]

Thank you very much for the answer. This is the "main" of the project. Tell me if you need also other associated files.

include <dart/dart.hpp>

include <dart/utils/urdf/DartLoader.hpp>

include <dart/dynamics/SmartPointer.hpp>

include "MyWindow.hpp"

include

include

// Set initial parameters about roll-pitch-yaw orientations (OUT OF MAIN) void setInitialConfiguration(dart::dynamics::SkeletonPtr naoRobot) { naoRobot->setPosition(5,0.35); // initial height //naoRobot->setPosition(5,0.6); // initial height

naoRobot->setPosition(naoRobot->getDof("HeadYaw")->getIndexInSkeleton(), 0 );
naoRobot->setPosition(naoRobot->getDof("HeadPitch")->getIndexInSkeleton(), 0 );
naoRobot->setPosition(naoRobot->getDof("RShoulderPitch")->getIndexInSkeleton(), 80*M_PI/180 );
naoRobot->setPosition(naoRobot->getDof("RShoulderRoll")->getIndexInSkeleton(), -10*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("RElbowYaw")->getIndexInSkeleton(), 50*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("RElbowRoll")->getIndexInSkeleton(), 2*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("RWristYaw")->getIndexInSkeleton(), 0 );
naoRobot->setPosition(naoRobot->getDof("LShoulderPitch")->getIndexInSkeleton(), 80*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("LShoulderRoll")->getIndexInSkeleton(), 10*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("LElbowYaw")->getIndexInSkeleton(), -50*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("LElbowRoll")->getIndexInSkeleton(), -2*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("LWristYaw")->getIndexInSkeleton(), 0 );

naoRobot->setPosition(naoRobot->getDof("RHipYawPitch")->getIndexInSkeleton(), 0 );
naoRobot->setPosition(naoRobot->getDof("RHipRoll")->getIndexInSkeleton(), 0 );
naoRobot->setPosition(naoRobot->getDof("RHipPitch")->getIndexInSkeleton(), -20*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("RKneePitch")->getIndexInSkeleton(), 34*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("RAnklePitch")->getIndexInSkeleton(), -14*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("RAnkleRoll")->getIndexInSkeleton(), 0 );

naoRobot->setPosition(naoRobot->getDof("LHipYawPitch")->getIndexInSkeleton(), 0 );
naoRobot->setPosition(naoRobot->getDof("LHipRoll")->getIndexInSkeleton(), 0 );
naoRobot->setPosition(naoRobot->getDof("LHipPitch")->getIndexInSkeleton(), -20*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("LKneePitch")->getIndexInSkeleton(), 34*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("LAnklePitch")->getIndexInSkeleton(), -14*M_PI/180.0 );
naoRobot->setPosition(naoRobot->getDof("LAnkleRoll")->getIndexInSkeleton(), 0 );

}

int main(int argc, char* argv[]) { // Array of names of main robot components' orientation std::string joints[] = {"HeadYaw", "HeadPitch", "LHipYawPitch", "LHipRoll", "LHipPitch", "LKneePitch", "LAnklePitch", "LAnkleRoll", "LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw", "RHipYawPitch", "RHipRoll", "RHipPitch", "RKneePitch", "RAnklePitch", "RAnkleRoll", "RShoulderPitch", "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw"};

// Create and initialize the world dart::simulation::WorldPtr world(new dart::simulation::World); assert(world != nullptr); char full_path1 = realpath("ground.urdf", NULL); char full_path2 = realpath("nao.urdf", NULL); std::string path_ground(full_path1); std::string path_nao(full_path2); std::cout << full_path1 << std::endl; std::cout << full_path2 << std::endl;

// Load skeletons dart::utils::DartLoader dl; dart::dynamics::SkeletonPtr ground = dl.parseSkeleton(path_ground); dart::dynamics::SkeletonPtr naoRobot = dl.parseSkeleton(path_nao);

// In SERVO mode still velocity is input, output is joint acceleration. The constraint solver will try to track the desired velocity within the joint force limit // In practice we are simulating a real servo motor for (int i=0; i<24; ++i){ naoRobot->getJoint(joints[i])->setActuatorType(dart::dynamics::detail::SERVO);

}

//for (int i=0; i<24; ++i){ // naoRobot->getJoint(joints[i])->setActuatorType(dart::dynamics::detail::VELOCITY); //}

// for (int i=0; i<6; ++i){ // naoRobot->getJoint(i)->setActuatorType(dart::dynamics::detail::FORCE); // }

double sufficient_force = 1e+5; double posUpperLimit = 286.14; double posLowerLimit = -286.14;

/*for (int i=0; i<24; i++) { naoRobot->getJoint(joints[i])->setForceUpperLimit(0, sufficient_force); naoRobot->getJoint(joints[i])->setForceLowerLimit(0, -sufficient_force);

  naoRobot->getJoint(joints[i])->setDampingCoefficient(0, 0.0);
  naoRobot->getJoint(joints[i])->setSpringStiffness(0, 0.0);
  naoRobot->getJoint(joints[i])->setCoulombFriction(0, 0.0);
  naoRobot->getJoint(joints[i])->setPositionLimitEnforced(true);

  naoRobot->getJoint(joints[i])->setPositionUpperLimit(0, posUpperLimit);
  naoRobot->getJoint(joints[i])->setPositionLowerLimit(0, posLowerLimit);

  naoRobot->getJoint(joints[i])->setVelocityUpperLimit(0, posUpperLimit);
  naoRobot->getJoint(joints[i])->setVelocityLowerLimit(0, posLowerLimit);

  naoRobot->getJoint(joints[i])->setAccelerationUpperLimit(0, posUpperLimit);
  naoRobot->getJoint(joints[i])->setAccelerationLowerLimit(0, posLowerLimit);

}

// Use the function setInitialConfiguration explained on top setInitialConfiguration(naoRobot);

world->addSkeleton(ground); world->addSkeleton(naoRobot);

//dart::dynamics::Shape line(dart::dynamics::Shape::LINE_SEGMENT);

// Create and initialize the world: gravity //Eigen::Vector3d gravity(0.0, 0.0, -9.81); Eigen::Vector3d gravity(0.0, 0.0, 0.0);

world->setGravity(gravity);

// And timestep world->setTimeStep(2.0/100);

// Create puntatori supportFoot and swingFoot of BodyNode class //dart::dynamics::BodyNode supportFoot = naoRobot->getBodyNode("l_wrist"); //dart::dynamics::BodyNode swingFoot = naoRobot->getBodyNode("r_wrist");

dart::dynamics::BodyNode supportFoot = naoRobot->getBodyNode("l_sole"); dart::dynamics::BodyNode swingFoot = naoRobot->getBodyNode("r_sole");

// Call MyWindow constructor (MyWindow.cpp) passing as argument Controller constructor (Controller.cpp) // The order of execution is: Controller, MyWindow MyWindow window(new Controller(naoRobot, supportFoot, swingFoot, world)); // -> Controller.cpp & -> MyWindow.cpp window.setWorld(world);

[mxgrey]

To get better formatting, I recommending copy/pasting your code into a gist, saving it, and then linking it here. Be sure to select "Create public gist".

[pet93]

Thank you for the suggestion. Here the link: https://gist.github.com/pet93/228c7d57033534aa2bf405cb010eccac#file-gistfile1-txt

[mxgrey]

I'm afraid that the description and code you've provided so far doesn't offer us much insight into the problem that you're having. The following information would enable us to give you more meaningful guidance:

• A more detailed description of what the simulation is doing (preferably a video or screen recording of the simulation).
• A detailed explanation of what about the simulation results do not match your expectations.
• A link to your entire source code, or a complete minimal working example of the source code. We would also need access to the robot model that you are using in order to examine this ourselves. As it currently stands, I have no idea what your implementation of MyWindow might be doing.

[pet93]

Hi mxgrey! I have updated all codes concerning the project on my profile in NAO repository. My project concerns the NAO humanoid's motions and it works by setting VELOCITY actuation type. Once switched to SERVO actuators robot doesn't move as I desired, e.g., I ask robot to move its left leg and it doesn't move at all. Moreover it falls down due to gravity. For the gravity issue I've changed robot force limits and after this it stands but still doesn't move as I want. My question concerns the possibility to set further parameters to remove or to smooth joint limits and make robot moves as desired. Main codes are controller.cpp and main.cpp. Thank you very much for pay attention

[mxgrey]

I looked through the NAO repo on your Github profile. It looks like you're calling Skeleton::setVelocity(~) to set the SERVO command. Unfortunately, that is only supported for VELOCITY command mode. The reason is that the setVelocity(~) command is meant to literally set the current velocity of the generalized coordinate (i.e. it will change the state of the robot), but we cannot guarantee that the velocity you specify can actually be achieved in SERVO mode, since SERVO mode is restricted by joint limits and other physical constraints. If you are in VELOCITY mode, you can use setVelocity(~) to set both the state and the velocity command at the same time, because VELOCITY mode will happily violate physics in order to achieve the requested velocity, but that is not permitted for SERVO mode.

In general, the recommended way to set an actuator command is to use the Skeleton::setCommand(~) or Joint::setCommand(~) function. That will correctly set the actuator command no matter what actuator mode you are using. Since you are not calling Joint::setCommand(~), the simulator will default to using a command of 0, and the SERVO actuator will therefore try to achieve zero velocity all the time at those actuated joints, which will likely cancel out your call to Skeleton::setVelocity(~) which would have otherwise changed the velocity of the system.

So try to change your setVelocity calls to setCommand in Controller.cpp and see if that helps.

[pet93]

Thank you very much for the answer, now it works!!! You have been so kind! Last little question and then I've finished: it is normal that by setting SERVO actuator the x and y coordinates concerning CoM Jacobian have the opposite direction with respect to CoM Jacobian x and y coordinates in VELOCITY actuator? Because now if I try to move CoM toward a specific direction/point in the "x-y plane" it moves in the opposite direction. Thank you for paying attention!

[mxgrey]

The joint actuator type has no effect on the CoM Jacobian. It's plausible that there may be a sign error in the implementation of the SERVO actuator type where it's treating the command as the negative of the desired velocity (which would obviously be a bad convention). I have to confess, I haven't use the SERVO mode myself, so this may have been going on without me noticing.

It would be good to have a unit test to ensure that SERVO commands and VELOCITY commands produce the same result in the absence of any constraints, if we don't have such a test already. I'll put together a test for that when I get a chance.

[jslee02]

We actually have a test for it.

It would be always great to provide an executable test code that reproduces the unexpected behavior so that we can diagnosis.

[pet93]

I've made different tests and finally the problem is not related to SERVO actuation nor to setCommand. I don't know why, but when I compute getCOMLinearJacobian and I use it entirely, its reference frame is the world one as expected; while if I extract some column as I made with JCoML and JCoMR (in Controller.cpp in the Jacobians' computation) to make the CoM is moved only by left or right foot, x and y direction are opposite (maybe also z, I'll test later). Therefore I've changed the sign of first and second column of JCoML and JCoMR and it works correctly on x-y plane but I don't know why this happens!

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed in 7 days if no further activity occurs. Thank you for your contributions.