The setExternalForce() method

[christophecricket]

Hi

I'm getting unpredictable behaviour in my simulation, and it seems the only statement in my code that has ambiguous meaning is the setExternalForce() that I applied to one of my objects. What is the localidx parameter that has to be applied to it?

Nothing seems to change no matter the integer I pass to it, but if it's in the method, I assume it serves some purpose.

[jhwangbo]

are you simulating a single body? Then it actually serves no purpose. It is there so that we can apply force on both articulated systems and single bodies with the same method.

What behaviour do you see? Have you tested it with valgrind?

[christophecricket]

I have 2 SingleBodyObjects. The first object is connected to the fixed world with a spring and a damper, and connected to the second object by another spring and damper. I'm imposing a position on the second object and trying to get a realistic reading on the force the first object is exerting on the second. My problem is that I'm not getting realistic force outputs at certain frequencies. I've never used valgrind before but it seems like a debugging tool for memory leaks. Do you think it would help me in my use case?

Here's my code for reference. If you ignore the ROS stuff it should be pretty clear.

#include "raisim/World.hpp"
#include <iostream>
#include <Eigen/Core>
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "geometry_msgs/Vector3.h"
#include <vector>
#include <cmath>

int main(int argc, char **argv) {
	
    raisim::World world;
    world.setDefaultMaterial(0,0,0);
    
    //define objects, addBox argument list: xdim, ydim, zdim, mass, (collision options)
 	raisim::Box* box_m = world.addBox(0.001,0.001,0.001,1,"default",0,CollisionGroup(1));
 	raisim::Box* box_i = world.addBox(0.001,0.001,0.001,1,"default",0,CollisionGroup(1));
 	box_m->setBodyType(raisim::BodyType::DYNAMIC);
 	box_i->setBodyType(raisim::BodyType::KINEMATIC);

 	//turn gravity off
 	raisim::Vec<3> gravity;
 	gravity[0] = 0;
 	gravity[1] = 0;
 	gravity[2] = 0;
	world.setGravity(gravity);

	//configure simulation
	world.setTimeStep(0.001);
	int iteration_counter = 0;

	//declare forces
	raisim::Vec<3> fix_force;//spring-damper force from object to fixed world
	raisim::Vec<3> free_force;//spring-damper force from object to interaction point

	//declare positions and velocities
	Eigen::Vector3d pos_m;
	Eigen::Vector3d vel_m;
	Eigen::Vector3d pos_i;
	Eigen::Vector3d vel_i;

	//declare and define parameters
	Eigen::Vector3d free_stiff;
	free_stiff << 1,1,1;
	Eigen::Vector3d fix_stiff;
	fix_stiff << 1,1,1;
	Eigen::Vector3d free_damp;
	free_damp << 1,1,1;
	Eigen::Vector3d fix_damp;
	fix_stiff << 1,1,1;

	//start ROS node
	ros::init(argc, argv, "raisim");
	ros::NodeHandle n;

	ros::Publisher force_pub = n.advertise<geometry_msgs::Vector3>("/raisim/force", 1000);
	geometry_msgs::Vector3 msg;
	std::vector<double> msg_placeholder;
	msg_placeholder.resize(3);
	ros::Rate loop_rate(1000);

	//trajectory parameters
	double w = 1;

	while(ros::ok()){

		//sine input
		box_i->setVelocity(1*w*cos(w*iteration_counter*0.001),0,0,0,0,0);

		pos_m = box_m->getPosition();
		vel_m = box_m->getLinearVelocity();
		pos_i = box_i->getPosition();
		vel_i = box_i->getLinearVelocity();

		fix_force[0] = - fix_stiff[0]*pos_m[0] - fix_damp[0]*vel_m[0]
						- free_stiff[0]*(pos_m[0]-pos_i[0]) 
						- free_damp[0]*(vel_m[0]-vel_i[0]);

		fix_force[1] = - fix_stiff[1]*pos_m[1] - fix_damp[1]*vel_m[1]
						- free_stiff[1]*(pos_m[1]-pos_i[1]) 
						- free_damp[1]*(vel_m[1]-vel_i[1]);

		fix_force[2] = - fix_stiff[2]*pos_m[2] - fix_damp[2]*vel_m[2]
						- free_stiff[2]*(pos_m[2]-pos_i[2]) 
						- free_damp[2]*(vel_m[2]-vel_i[2]);

		box_m->setExternalForce(0,fix_force);

		std::cout << "Time: " << iteration_counter*0.001 << " | Force: " << 
			pos_i[0] << std::endl;

	    world.integrate();

	    msg_placeholder.at(0) = - free_stiff[0]*(pos_m[0]-pos_i[0]) 
								- free_damp[0]*(vel_m[0]-vel_i[0]);

		msg_placeholder.at(1) = - free_stiff[1]*(pos_m[1]-pos_i[1]) 
								- free_damp[1]*(vel_m[1]-vel_i[1]);

		msg_placeholder.at(2) = - free_stiff[2]*(pos_m[2]-pos_i[2]) 
								- free_damp[2]*(vel_m[2]-vel_i[2]);

		msg.x = msg_placeholder.at(0);
		msg.y = msg_placeholder.at(1);
		msg.z = msg_placeholder.at(2);
		force_pub.publish(msg);

	    ++iteration_counter;
	    //raisim::MSLEEP(1);
	    loop_rate.sleep();
	}
}

[christophecricket]

Maybe this is an issue of my system having the same amount of zeros as poles? Would that be a problem? I don't know how well explicit solvers deal with these types of systems.

[jhwangbo]

If you say that it is frequency dependent, it is most likely that you are simulating an unstable system. Even if you are using a damper, your discrete system can be unstable.

It will be more stable if RaiSim can simulate continuous-time springs and dampers. I'll leave this as my todo list.

[jhwangbo]

You can also try with different time steps to see how the behavior changes.

[christophecricket]

The issue is actually that I'm getting a very low amplitude response at an excitation frequency of 1 rad/s. I'm expecting a magnitude of 0.69, and I'm getting something below 0.002. Do you still think it's an instability related to discretisation?

[christophecricket]

I'll try reducing the step size. Maybe my simulation starts converging

[christophecricket]

Update: increasing the step size by a factor of a 100 doesn't seem to help that much, but what I see that for frequencies below ~0.5 rad/s and above ~3 rad/s, the amplitude response is pretty good. It just seems that there's a problem somewhere in this range of frequencies. I think this shouldn't be a problem for me if I just insert more realistic values to my use case in the transfer function.

I'm excited to see the functionality you describe implemented, though! Thank you for the quick replies :)