## Robot jumps to path at simulation start

Typically: "How do I... ", "How can I... " questions
tumsimon
Posts: 8
Joined: 17 Jul 2014, 11:28

### Robot jumps to path at simulation start

Hello,
I am trying to make my robot move along a predefined path. Before starting the simulation the robot is always in the same position. When I start the simulation, the robot "jumps" to the beginning of the path, moves along the path, and jumps back as soon as the simulation ends. I want it to move to the path at the beginning and move back at the end.
That's why I thought I'd insert the current position of the robot's tip to the path once in the beginning and once in the end.

Two questions came up:

1. I get the position like that:

Code: Select all

spitze=simGetObjectHandle('tip')
--extend path to current position
akpos=simGetObjectPosition(spitze,-1)
akor=simGetObjectOrientation(spitze,-1)


But this does not give me the position, that the robot is in before I start the simulation. The path now begins somewhere else, but not where I want it to begin. What is the reason for that?

2. How can I get the number of ctrl-points in a path or how can I add a ctrl-point to the end of a path?

Thank you!

Simon

coppelia
Posts: 7441
Joined: 14 Dec 2012, 00:25

### Re: Robot jumps to path at simulation start

Hi Simon,

I would not modify the path control points. I would rather divide the movement in two distinct sequences:
• Move from the start position to the beginning of the path
• Move along the path
You could use something similar:

Code: Select all

startPosition=simGetObjectPosition(objectHandle,-1)
startOrientation=simGetObjectOrientation(objectHandle,-1)
startMatrix=simBuildMatrix(startPosition,startOrientation)

pathStartPosition=simGetPositionOnPath(pathHandle,0)
pathStartOrientation=simGetOrientationOnPath(pathHandle,0)
pathStartMatrix=simBuildMatrix(pathStartPosition,pathStartOrientation)

-- Then interpolate from the two matrices. t is the interpolation factor (0=matrix1, 1=matrix2)
matr=simInterpolateMatrices(startMatrix,pathStartMatrix,t)
interpolatedPosition={matr[4],matr[8],matr[12]}
interpolatedorientation=simGetEulerAnglesFromMatrix(matr)
Or you could use the simRMLMoveToPosition from a threaded child script.

Cheers

tumsimon
Posts: 8
Joined: 17 Jul 2014, 11:28

### Re: Robot jumps to path at simulation start

Thank you, that really helped!

I split the movement into three parts now: Moving to the path, moving along the path, and moving back to the original position. I used the matrix interpolation, which worked very nicely.

Now my only other problem is the joint positions. Of course, in the end they are different from in the beginning and when the simulation stops, you can still see them "jump" or "turn". Is there any way to avoid that?

coppelia
Posts: 7441
Joined: 14 Dec 2012, 00:25

### Re: Robot jumps to path at simulation start

If I understood you correctly, your robot is a redundant kinematic chain that you are driving via IK from its start configuration, to the beginning of a path, along of that path to the end of the path. From there, you want to return to the start configuration.

You could simply do an interpolation between two configurations, but then your end-ffector would not move on a straight line.

Or you could use following API function: simGetMpConfigTransition (but then you'll have to build a motion planning object, and the function is a bit complicated to be used in an efficient way).

Cheers

tumsimon
Posts: 8
Joined: 17 Jul 2014, 11:28

### Re: Robot jumps to path at simulation start

You understood correctly. The first suggestion sounds great. The end-effector does not have to move on a straight line.
But how do I do it? Do I use simGetObjectConfiguration to get the startConfiguration? That returns a rawBufferHandle, how can I make a matrix out of that? Should I use simInterpolateMatrices again?

Thanks so much for your help!

coppelia
Posts: 7441
Joined: 14 Dec 2012, 00:25

### Re: Robot jumps to path at simulation start

You would do it in a way similar to:

Code: Select all

-- In the initialization phase:
robotJointHandles={}
for i=1,6,1 do -- in case of a 6 DoF robot
robotJointHandles[i]=simGetObjectHandle('robotJoint'..i) -- put appropriate names here
end

-- Get the start configuration:
startConfiguration={}
for i=1,#robotJointHandles,1 do
startConfiguration[i]=simGetJointPosition(robotJointHandles[i])
end

-- Once you have arrived at the end of the path, get the configuration there:
endOfPathConfiguration={}
for i=1,#robotJointHandles,1 do
endOfPathConfiguration[i]=simGetJointPosition(robotJointHandles[i])
end

-- Now you can interpolate between those 2 configurations via the t parameter (variying between 0 and 1)
-- t=0 gives us the 'endOfPathConfiguration, t=1 gives us the 'startConfiguration'
config={}
for i=1,#robotJointHandles,1 do
config[i]=endOfPathConfiguration[i]*(1-t)+startConfiguration[i]*t
end

-- Now you can apply this to the robot:
for i=1,#robotJointHandles,1 do
simSetJointPosition(robotJointHandles[i],config[i])
end
Cheers