Hello,
In the initial state I have adjusted the joint angle of the robot. Make the end of the manipulator face down. But when the manipulator moves, he cannot maintain a stable posture. It is possible to make the grasped object face the sky. How can he keep his posture unchanged and relatively stable?
Should I adjust the direction of Dummy, I don’t know?
The end of the robot moves along the path I have set. There is no dummy in the path, only the starting point and the ending point.
start position
during moving
best regards
shaoxiang
How to control the robot to maintain a stable posture?
-
- Posts: 18
- Joined: 23 May 2020, 15:39
-
- Posts: 18
- Joined: 23 May 2020, 15:39
Re: How to control the robot to maintain a stable posture?
I used
Here is my code.
sim.getPositionOnPath
, sim.getOrientationOnPath
, sim.setObjectPosition
, sim.setObjectOrientation
.Here is my code.
Code: Select all
-- do some initialization here
-- Get handles and postions of dummies
tip = sim.getObjectHandle("tip")
ownHandle = sim.getObjectHandle("Target")
startPath1=sim.getObjectHandle("Path1_1")
startPath2=sim.getObjectHandle("Path1_2")
mittelPath1=sim.getObjectHandle("Path2_1")
mittelPath2=sim.getObjectHandle("Path2_2")
endPath1=sim.getObjectHandle("Path3_1")
endPath2=sim.getObjectHandle("Path3_2")
pathLength1=sim.getPathLength(startPath1)
pathLength2=sim.getPathLength(startPath2)
pathLength3=sim.getPathLength(mittelPath1)
pathLength4=sim.getPathLength(mittelPath2)
pathLength5=sim.getPathLength(endPath1)
pathLength6=sim.getPathLength(endPath2)
speed=0.02 -- end-effector velocity in m/s
sim.setUserParameter(ownHandle,'@enable','')
timestep=sim.getSimulationTimeStep() -- in s
currentPositionRelative1=0
currentPositionRelative2=0
currentPositionRelative3=0
currentPositionRelative4=0
currentPositionRelative5=0
currentPositionRelative6=0
moveForward=true
epsilon1=0.01
epsilon2=0.01
epsilon3=0.01
epsilon4=0.01
epsilon5=0.01
epsilon6=0.01
function sysCall_threadmain()
-- Put some initialization code here
while sim.getSimulationState()~=sim.simulation_advancing_abouttostop do
slowerWork=sim.getStringSignal("slowerWork")
if slowerWork=="1" then
slowerWork=true
else
slowerWork=false
end
-- path1
if not slowerWork then
pathAdvanceAbsolute=speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength1
if currentPositionRelative1 < 1-epsilon1 and moveForward then
currentPositionRelative1=currentPositionRelative1+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(startPath1,currentPositionRelative1)
pathOrientation=sim.getOrientationOnPath(startPath1,currentPositionRelative1)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
else
pathAdvanceAbsolute=0.2*speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength1
if currentPositionRelative1 < 1-epsilon1 and moveForward then
currentPositionRelative1=currentPositionRelative1+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(startPath1,currentPositionRelative1)
pathOrientation=sim.getOrientationOnPath(startPath1,currentPositionRelative1)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
end
-- path2
if currentPositionRelative1 >= 1-epsilon1 then
if not slowerWork then
pathAdvanceAbsolute=speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength2
if currentPositionRelative2 < 1-epsilon2 and moveForward then
currentPositionRelative2=currentPositionRelative2+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(startPath2,currentPositionRelative2)
pathOrientation=sim.getOrientationOnPath(startPath2,currentPositionRelative2)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
else
pathAdvanceAbsolute=0.2*speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength2
if currentPositionRelative2 < 1-epsilon2 and moveForward then
currentPositionRelative2=currentPositionRelative2+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(startPath2,currentPositionRelative2)
pathOrientation=sim.getOrientationOnPath(startPath2,currentPositionRelative2)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
end
end
-- path3
if currentPositionRelative2 >= 1-epsilon2 then
if not slowerWork then
pathAdvanceAbsolute=speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength3
if currentPositionRelative3 < 1-epsilon3 and moveForward then
currentPositionRelative3=currentPositionRelative3+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(mittelPath1,currentPositionRelative3)
pathOrientation=sim.getOrientationOnPath(mittelPath1,currentPositionRelative3)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
else
pathAdvanceAbsolute=0.2*speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength3
if currentPositionRelative3 < 1-epsilon3 and moveForward then
currentPositionRelative3=currentPositionRelative3+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(mittelPath1,currentPositionRelative3)
pathOrientation=sim.getOrientationOnPath(mittelPath1,currentPositionRelative3)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
end
end
-- path4
if currentPositionRelative3 >= 1-epsilon3 then
if not slowerWork then
pathAdvanceAbsolute=speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength4
if currentPositionRelative4 < 1-epsilon4 and moveForward then
currentPositionRelative4=currentPositionRelative3+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(endPath1,currentPositionRelative4)
pathOrientation=sim.getOrientationOnPath(endPath1,currentPositionRelative4)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
else
pathAdvanceAbsolute=0.2*speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength4
if currentPositionRelative4 < 1-epsilon4 and moveForward then
currentPositionRelative4=currentPositionRelative4+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(endPath1,currentPositionRelative4)
pathOrientation=sim.getOrientationOnPath(endPath1,currentPositionRelative4)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
end
end
-- path5
if currentPositionRelative4 >= 1-epsilon4 then
if not slowerWork then
pathAdvanceAbsolute=speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength5
if currentPositionRelative5 < 1-epsilon5 and moveForward then
currentPositionRelative5=currentPositionRelative5+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(endPath2,currentPositionRelative5)
pathOrientation=sim.getOrientationOnPath(endPath2,currentPositionRelative5)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
else
pathAdvanceAbsolute=0.2*speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength5
if currentPositionRelative5 < 1-epsilon5 and moveForward then
currentPositionRelative5=currentPositionRelative5+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(endPath2,currentPositionRelative5)
pathOrientation=sim.getOrientationOnPath(endPath2,currentPositionRelative5)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
end
end
-- path6
if currentPositionRelative5 >= 1-epsilon5 then
if not slowerWork then
pathAdvanceAbsolute=speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength6
if currentPositionRelative6 < 1-epsilon6 and moveForward then
currentPositionRelative6=currentPositionRelative6+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(mittelPath2,currentPositionRelative6)
pathOrientation=sim.getOrientationOnPath(mittelPath2,currentPositionRelative6)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
else
pathAdvanceAbsolute=0.2*speed*timestep -- in m
pathAdvanceRelative=pathAdvanceAbsolute/pathLength6
if currentPositionRelative6 < 1-epsilon6 and moveForward then
currentPositionRelative6=currentPositionRelative6+pathAdvanceRelative
pathPosition=sim.getPositionOnPath(mittelPath2,currentPositionRelative6)
pathOrientation=sim.getOrientationOnPath(mittelPath2,currentPositionRelative6)
objectPositionIsSet = sim.setObjectPosition(ownHandle,-1,pathPosition)
objectOrientationIsSet = sim.setObjectOrientation(ownHandle,-1,pathOrientation)
end
end
end
if currentPositionRelative6 >= 1-epsilon6 then
currentPositionRelative1=0
currentPositionRelative2=0
currentPositionRelative3=0
currentPositionRelative4=0
currentPositionRelative5=0
currentPositionRelative6=0
end
end
end
function sysCall_cleanup()
-- Put some clean-up code here
end
-- See the user manual or the available code snippets for additional callback functions and details
Re: How to control the robot to maintain a stable posture?
is the orientation of the path control point correct?
-
- Posts: 18
- Joined: 23 May 2020, 15:39
Re: How to control the robot to maintain a stable posture?
Thanks a lot for your tips! I adjust the orientation of point in the path. Now it is better!
We can see ,the Z axis (blue axis)of TargetDummy face ground. But the tip(another blue axis) don't follow it. I don't know why .
We can see ,the Z axis (blue axis)of TargetDummy face ground. But the tip(another blue axis) don't follow it. I don't know why .
Re: How to control the robot to maintain a stable posture?
Made a quick test using these functions and it seems to work as expected:Shaoxiang Wang wrote: ↑01 Jun 2020, 21:56 I usedsim.getPositionOnPath
,sim.getOrientationOnPath
,sim.setObjectPosition
,sim.setObjectOrientation
.
Code: Select all
function sysCall_init()
Path=sim.getObjectHandle('Path')
manipSphere=sim.getObjectHandle('redundantRob_manipSphere')
end
function sysCall_actuation()
local t=math.fmod(sim.getSimulationTime()*0.1,1)
local p=sim.getPositionOnPath(Path,t)
local o=sim.getOrientationOnPath(Path,t)
sim.setObjectPosition(manipSphere,-1,p)
sim.setObjectOrientation(manipSphere,-1,o)
end
Re: How to control the robot to maintain a stable posture?
also: you are using a damped resolution method (DLS): it will take more iterations to converge to the desired end-effector position/orientation. Either reduce the damping factor, or use a non-damped resolution method (i.e. pseudo-inverse)
Cheers
Cheers
-
- Posts: 18
- Joined: 23 May 2020, 15:39
Re: How to control the robot to maintain a stable posture?
Thank you very much! Now it works well! Thank!