How to set velocity directly?

Typically: "How do I... ", "How can I... " questions
Post Reply
skiesel
Posts: 10
Joined: 18 May 2015, 17:40

How to set velocity directly?

Post by skiesel » 18 May 2015, 23:12

Hi,

I'm wondering if there is a way to set the velocity directly of a joint. For example, if I use "simxSetJointTargetVelocity" there is a period of acceleration, and eventually the joint reaches the target velocity. Of course there are the various velocities that occur during acceleration, and you can see them using "simxGetObjectVelocity".

It's very clear how to set the position and orientation, but it is unclear, seemingly not possible to set the velocity directly?

The reason I ask is because I'd like to be able to use V-REP as part of an external motion planning library. I really need to be able to capture the entire state of the robot (or also environment if the environment is non-static) and be able to recreate it later. As I said before, this all seems very easy except for velocity.

I looked through the remote and regular apis but didn't find anything that seemed like it was capable of something like this?

Thanks in advance for any help! (I also am aware that you provide built in motion planning but it didn't look like there were any api hooks available or tutorials for adding a new motion planner).

DannyYeong
Posts: 44
Joined: 24 Feb 2015, 17:06

Re: How to set velocity directly?

Post by DannyYeong » 19 May 2015, 08:20

Have you tried changing the joint's mode to "joints in motion mode"? Or it's something different you`re looking for

coppelia
Site Admin
Posts: 7272
Joined: 14 Dec 2012, 00:25

Re: How to set velocity directly?

Post by coppelia » 19 May 2015, 11:11

Hello,

are you trying to implement a motion/path planning algorithm via a remote API client? This works, but is more difficult and troublesome than the alternative to use a plugin. With a plugin, you can register custom script function that will be able to call-back your code, giving you much more flexibility and speed (since there is no communication delay).

Then, currently a joint that is dynamically enabled will always start a velocity 0. During simulation, changing instantaneously that velocity is not possible, since this would also not be possible in reality. Also, setting a joint's velocity to a certain value will not directly work, since the attached masses should also have a correct initial linear and angular velocity (i.e. the joint's velocity does not exist per se, it is given by the velocity relationship of the attached bodies).

You can specify initial dynamic linear/angular velocities for a shape with simSetObjectFloatParameter and parameter ids 3000-3002, and 3020-3022.

Are you sure you need the dynamics for your path/motion planning? Otherwise you could simply control the position of your joints instantaneously, and handle the acceleration yourself in a child script for instance.

Cheers

skiesel
Posts: 10
Joined: 18 May 2015, 17:40

Re: How to set velocity directly?

Post by skiesel » 19 May 2015, 11:21

Thank you so much for your response. In your builtin motion planning functionality, are you then only planning a positional path and then relying on a controller to follow that path?

I originally started with the remote API and then moved towards using a plugin.

I understand that it's a little more complicated than simply setting the velocity of one joint (and not the bodies attached). I was really hoping to serialize the whole state of the robot's tree.

In terms of needing the dynamics, I think we do unfortunately. Without also planning in the velocity space, we could come up with positional paths that would be dynamically infeasible. Imagine something like a car with limited horse power trying to climb a steep hill. There are no positional constraints that would disallow the car from climbing the hill, however, the car may need to get a "running start" or it may just not be possible to climb the hill given the environment. The motion planner would need to be able to take this into account and find a way around. Does that make sense?

coppelia
Site Admin
Posts: 7272
Joined: 14 Dec 2012, 00:25

Re: How to set velocity directly?

Post by coppelia » 20 May 2015, 08:59

Yes, that makes perfect sense, but in that case, you simply need to know the limits of your system (e.g. max. velocity, max. accelerations, force/torque..), but why do you want to apply them during path planning? Path planning would take a collection of input data (e.g. collision geometries, start/goal configurations, joint limits, max. velocity, max. accel, etc.) and do calculations based on that, without the need of a physics engine. Or did I misunderstand something? Is your path planning algorithm actively using the calculations produced by the physics engine?

Cheers

skiesel
Posts: 10
Joined: 18 May 2015, 17:40

Re: How to set velocity directly?

Post by skiesel » 20 May 2015, 11:39

Again, thank you so much for your response. I am using the calculations from the physics engine during motion planning. I am interested in motion planning in situations where doing the calculations "by hand" would be very complicated, so I would rely on the physics engine to do that for me. For example, driving a buggy car over rough terrain. Modeling the interaction with the tires, suspension and ground is much easier done by relying on a physics engine.

I think I have been able to get what I need from your earlier suggestion by setting the initial velocities of the various objects in the model tree.

Code: Select all

void saveState(simInt p3dxHandle, simChar **poses, std::vector<double> &velocities) {
	simInt objectCount = 0;
	simInt* objectTree = simGetObjectsInTree(p3dxHandle, sim_handle_all, 0, &objectCount);	
	*poses = simGetConfigurationTree(p3dxHandle);

	simFloat velocity[6];
	for(unsigned int i = 0; i < objectCount; i++) {
		simInt handle = objectTree[i];
		simGetObjectVelocity(handle, velocity, &velocity[3]);

		for(unsigned int j = 0; j < 6; j++) {
			velocities.push_back(velocity[j]);
		}
	}
}

void loadState(simInt p3dxHandle, simChar *poses, const std::vector<double> &velocities) {
	simInt objectCount = 0;
	simInt* objectTree = simGetObjectsInTree(p3dxHandle, sim_handle_all, 0, &objectCount);

	simSetConfigurationTree(poses);

	for(unsigned int i = 0; i < objectCount; i++) {
		simInt handle = objectTree[i];
		unsigned int indexBase = i * 6;

		simSetObjectFloatParameter(handle, 3000, velocities[indexBase]);
		simSetObjectFloatParameter(handle, 3001, velocities[indexBase+1]);
		simSetObjectFloatParameter(handle, 3002, velocities[indexBase+2]);

		simSetObjectFloatParameter(handle, 3020, velocities[indexBase+3]);
		simSetObjectFloatParameter(handle, 3021, velocities[indexBase+4]);
		simSetObjectFloatParameter(handle, 3022, velocities[indexBase+5]);

		simResetDynamicObject(handle);
	}
}
I think the only thing that is missing from above, which I am trying to figure out now, is that I am setting the target velocities of the joints ("Pioneer_p3dx_leftMotor", "Pioneer_p3dx_rightMotor") to induce motion, and when I restore the state I believe I also need to update these target velocities.

coppelia
Site Admin
Posts: 7272
Joined: 14 Dec 2012, 00:25

Re: How to set velocity directly?

Post by coppelia » 20 May 2015, 16:27

Ah, ok, I now better understand what you are trying to achieve!

For the target velocity, simply apply the same target velocity as you read out with simGetJointTargetVelocity.

Not sure how well the method will work for you, you will have to try it out. You might also have to slightly recenter the wheels.. a physics engine will always have small errors in joints (i.e. the base and mobile axis of a joint can slightly be misaligned or shifted during simulation. That joint internal error can become more important at high speeds or when large masses interact).

Cheers

skiesel
Posts: 10
Joined: 18 May 2015, 17:40

Re: How to set velocity directly?

Post by skiesel » 20 May 2015, 16:34

Great! Thanks for all of your help. I'll post back to this thread after I do some more testing to let anyone else looking to do something similar know what the final solution was.

coppelia
Site Admin
Posts: 7272
Joined: 14 Dec 2012, 00:25

Re: How to set velocity directly?

Post by coppelia » 20 May 2015, 17:11

If you are doing this for the Pioneer robot model, this shouldn't be too difficult. There are not that many dynamic parts: body, left wheel, right wheel, caster element and caster wheel. For those shapes, you need to memorize the linear and angular velocities. For the wheel joints and the caster joint, you need to memorize the angular position, their target velocity. You also need to memorize the spacial position/orientation of the body.

Then, when you want to reset everything to a state that you read during simulation: stop the simulation, then put the body into the memorized spacial position/orientation, set the other shapes to their respective relative positions (relative to the joints they are attached), then set all initial linear/angular velocities, and set the memorized joint values.

If you want to not stop the simulation, then you will have to call simResetDynamicObject on all above mentioned shapes and joints, then set their memoriez states.

Cheers

Post Reply