- arg1 is the start position (along the path)
- arg2 is the goal position (along the path)
- arg3 is the max. allowed velocity
- arg4 is the max. allowed acceleration
- arg5 is the max. allowed jerk
Inquiry for follow-path code
Re: Inquiry for follow-path code
-
- Posts: 49
- Joined: 26 Oct 2021, 01:35
Re: Inquiry for follow-path code
I have two issues I currently need to address and I would like some advice again.
Firstly, is it possible to implement this code in a non-threaded child script, i.e. not in a
Secondly, I am having problems when I set my robot's respondable mesh-parts with dynamic mode enabled and with the joints in torque/force mode because I would like the robot to eventually walk on the floor, and it cannot do that if the parts are not dynamic, and when the parts are dynamic the joints cannot be passive like in the redundant.ttt scene because the parts would just fall apart at the joints.
When my robot is in dynamic mode, however, the robot falls on its side, it rolls over and the joints bend randomly at the same time. I looked up other questions on the forum, and I found that this odd behavior could possibly be fixed by adjusting the gravity. Would this work in my case probably?
Thank you in advance.
Firstly, is it possible to implement this code in a non-threaded child script, i.e. not in a
coroutineMain()
function?Code: Select all
while sim.getSimulationTime()<4 do
sim.switchThread()
end
-- follow path here
When my robot is in dynamic mode, however, the robot falls on its side, it rolls over and the joints bend randomly at the same time. I looked up other questions on the forum, and I found that this odd behavior could possibly be fixed by adjusting the gravity. Would this work in my case probably?
Thank you in advance.
Re: Inquiry for follow-path code
Yes, you can do everything also from a non-threaded child script, but you'll have to put some work into it. Smooth movements are conveniently generated with sim.moveToConfig and sim.moveToPose, but those functions are blocking (since they execute over a certain duration), so they can't be used from a non-threaded script. In that case, you should appropriately use sim.ruckigPos, sim.ruckigStep and sim.ruckigRemove.
If your robot is dynamically enabled, you'll probably want to have all your joints in position control. Additionally, you will have to adjust several values, make sure there are no self-collisions causing vibrations, etc. Make sure to carefully read this page about designing dynamic simulations.
Cheers
If your robot is dynamically enabled, you'll probably want to have all your joints in position control. Additionally, you will have to adjust several values, make sure there are no self-collisions causing vibrations, etc. Make sure to carefully read this page about designing dynamic simulations.
Cheers
-
- Posts: 49
- Joined: 26 Oct 2021, 01:35
Re: Inquiry for follow-path code
Is it possible to have position control and an Inverse Kinematics environment at the same time?
Re: Inquiry for follow-path code
of course. There are several example scenes related to this, for instance scenes/kinematics/smoothMovementInFkAndIk.ttt, which drives a robot arm in IK, then FK, by respecting max. velocity, max. acceleration, and max. jerk constraints.
Cheers
Cheers
-
- Posts: 49
- Joined: 26 Oct 2021, 01:35
Re: Inquiry for follow-path code
I'll be sure to check this scene out.
Another question - path related
Is it possible to generate or add path objects via code rather than doing "Add" > "Path" > "Open"/"Closed"? I would like to somehow loop the creation of, say, 40 versions of the same path object if it is possible. This is so that I can develop a gait for my biped.
Thanks in advance.
Another question - path related
Is it possible to generate or add path objects via code rather than doing "Add" > "Path" > "Open"/"Closed"? I would like to somehow loop the creation of, say, 40 versions of the same path object if it is possible. This is so that I can develop a gait for my biped.
Thanks in advance.
Re: Inquiry for follow-path code
Yes, sim.createPath does that.
But paths objects are often not needed, oftentimes, you are better off in generating the path data and auxiliarily displaying it in 3D if needed (via drawing objects). Path objects are more meant when you need to visually design a path.
Cheers
But paths objects are often not needed, oftentimes, you are better off in generating the path data and auxiliarily displaying it in 3D if needed (via drawing objects). Path objects are more meant when you need to visually design a path.
Cheers
-
- Posts: 49
- Joined: 26 Oct 2021, 01:35
Re: Inquiry for follow-path code
Hello. I come back again to ask for advice regarding this.coppelia wrote: ↑01 Feb 2022, 08:01 Hello,
sim.followPath is deprecated and only works with the old-type path objects. That function is still there for backward compatibility reasons, but old-type paths are not available directly anymore since CoppeliaSim V4.2
Use the new paths object, and use your own path following function. As an example, open the demo scene scenes/movingAlongAPath.ttt, and replace the code of the blue cube with following:
also about your code: it would do nothing anyway, since the condition sim.getSimulationTime()>4 won't be met, and your thread will immediately end. Instead it should be something like:Code: Select all
function sysCall_init() cube=sim.getObject('.') path=sim.getObject('/Path') pathData=sim.unpackDoubleTable(sim.readCustomDataBlock(path,'PATH')) local m=Matrix(#pathData//7,7,pathData) pathPositions=m:slice(1,1,m:rows(),3):data() pathQuaternions=m:slice(1,4,m:rows(),7):data() pathLengths,totalLength=sim.getPathLengths(pathPositions,3) velocity=0.04 -- m/s corout=coroutine.create(coroutineMain) end function sysCall_actuation() if coroutine.status(corout)~='dead' then local ok,errorMsg=coroutine.resume(corout) if errorMsg then error(debug.traceback(corout,errorMsg),2) end end end function coroutineMain() sim.setThreadAutomaticSwitch(false) followPath(0,totalLength*0.25,velocity,0.01,99) followPath(totalLength*0.25,-totalLength*0.25,velocity*2,0.01,99) followPath(-totalLength*0.25,2*totalLength,velocity*4,0.01,99) end function callback(config,vel,accel) local p=config[1] p=p%totalLength local pos=sim.getPathInterpolatedConfig(pathPositions,pathLengths,p) local quat=sim.getPathInterpolatedConfig(pathQuaternions,pathLengths,p,nil,{2,2,2,2}) sim.setObjectPosition(cube,path,pos) sim.setObjectQuaternion(cube,path,quat) end function followPath(startPos,endPos,vel,accel,jerk) sim.moveToConfig(-1,{startPos},{0},{0},{vel},{accel},{jerk},{endPos},{0},callback) end
Make sure to use CoppeliaSim V4.3, or at least V4.2! (but CoppeliaSim V4.3 just came out and introduced many new features and bug fixes)Code: Select all
function coroutineMain() sim.setThreadAutomaticSwitch(false) while sim.getSimulationTime()<4 do sim.switchThread() end follow path here end
Cheers
So I have decided to use this algorithm for my project now since it seems to prove to be the most code-efficient one for me to use so far, i.e. the least lines of codes possibly used to control my robot. I took a short break which explains why not much progress.
Anyway, is there a way I can have my object, in this case my target dummy, stop and stay on a specific part of the path for an amount of time?
I would like to have target traverse a part of the path with a line of code similar to this:
followPath(0,totalLength*0.25,velocity,0.01,99)
and then have it stay at the end for x seconds before having it move again further along the path.
I tried doing code like this:
followPath(totalLength*0.25,totalLength*0.25,velocity,0.01,99)
but it did not work.
Thanks for the advice in advance.
Warm regards!
Re: Inquiry for follow-path code
Have a look at this scene, that does exactly this (inspect the script attached to the yellow cube).
That scene is the scene that ships with CoppeliaSim, but in CoppeliaSim 4.3 and earlier, the yellow cube is not yet present.
Cheers
That scene is the scene that ships with CoppeliaSim, but in CoppeliaSim 4.3 and earlier, the yellow cube is not yet present.
Cheers
-
- Posts: 49
- Joined: 26 Oct 2021, 01:35
Re: Inquiry for follow-path code
Hello.
So, I still am having problems with my robot model shaking very erratically and falling when I turn dynamic mode on for the meshes and when I turn torque/force mode on for the joints. This prevents me from testing out properly the gait pattern I composed using IK and path-dummy object.
I was thinking maybe I have to give my robot a center of mass and then manipulate it somehow if it is possible on Coppeliasim. Is it possible to give a mobile robot a CoM?
My benchmark to how a mobile robot controlled by IK-groups is the Asti model that comes with the installment. How is it that that model - a mobile model with all joints in torque/force mode and respondable mesh parts in dynamic mode able to stand without behaving strangely.
Thanks for future advice on this.
So, I still am having problems with my robot model shaking very erratically and falling when I turn dynamic mode on for the meshes and when I turn torque/force mode on for the joints. This prevents me from testing out properly the gait pattern I composed using IK and path-dummy object.
I was thinking maybe I have to give my robot a center of mass and then manipulate it somehow if it is possible on Coppeliasim. Is it possible to give a mobile robot a CoM?
My benchmark to how a mobile robot controlled by IK-groups is the Asti model that comes with the installment. How is it that that model - a mobile model with all joints in torque/force mode and respondable mesh parts in dynamic mode able to stand without behaving strangely.
Thanks for future advice on this.