Hi everyone,
first of all a huge thanks for the development of DART!
Currently, I am implementing my own robot simulation for my PhD, but I am relatively new to rigid body physics simulation.
I am a little bit stuck on the following issue:
I try to setup a robot simulation where I can simulate the stiffness and damping behaviour in the robotâs workspace. Therefore, I created a robot with stiffness and damping properties and used the KUKA KR 5 sixx urdf model for my first trials. Everything works as expected, when I start the GLUT simulation via the SPACE bar: due to gravity or external forces, the robot âwigglesâ around.
Now I try to set the robotâs configuration to different poses in the workspace. Using the IK solver of the last body node, I am able to calculate the robotâs cofiguration for that pose and set the robot to that configuration (both via ->solveAndApply() ). I am also able to see (visually in the GLUT simulation) that the pose was calculated and set correctly.
But the problem now is twofold:
- When I execute the IK before I started the GLUT simulation, the robot always swings back to the initial pose and does not start to wiggle around in the configuration which was calculated and set by the IK solver.
- When I execute the IK after I started the GLUT simulation, it looks like the robot is âpushedâ to the calculated IK solution, but swings back like a spring to the initial configuration.
Now my question is:
What is the correct way to set the robotâs configuration during simulation? Alternatively, I would also be happy if I found a way to stop the GLUT simulation, calculate and set the next desired configuration and start the simulation again.
A simple, relative âmovementâ is done as followed:
void Robot::relativeTranslationTCP(double deltaTransX, double deltaTransY, double deltaTransZ)
{
Eigen::Isometry3d tf = this->getSkeletonPtr()->getBodyNode(6)->getTransform();
tf.translation() += Eigen::Vector3d(deltaTransX, deltaTransY, deltaTransZ);
this->_inverseKinematicsPtr->getTarget()->setTransform(tf);
this->getSkeletonPtr()->getBodyNode(6)->getIK()->solveAndApply();
}
If necessary, I can also upload a video from the behaviour.
What am I missing here?
Thanks so much for any help and best regards from Germany!
Max
Edit: For your information, I could imagine that I didnât understand the time stepping overwrite correctly, my function looks like this:
void Environment::timeStepping()
{
if (testJointForce > 1000)
{
testJointForce = 100.0;
this->_robotPtr->reset();
}
SimWindow::timeStepping();
}
and my keyboard function as followed:
void Environment::keyboard(unsigned char key, int x, int y)
{
Eigen::Vector6d tmp;
switch (key)
{
case '+':
testJointForce += 100.0;
this->applyTorqueToRobotJoint(1, testJointForce);
break;
case '-':
this->_robotPtr->relativeTranslationTCP(0.0, 0.0, 0.05);
break;
case '0':
this->_robotPtr->setTCP(0.3, 0, 0, M_PI, 0, 0);
break;
default:
SimWindow::keyboard(key, x, y);
}
}