Moveit to CoppeliaSim
Posted: 04 Oct 2021, 21:23
Hello,
I am trying to control Franka_Panda robot through Moveit.
I can receive all the Franka_Panda joint positions in Coppeliasim with the following child script:
In the above script, I print the value of the first join position; however, I can do it for all joints.
The example script with FrankaEmikaPanda (embedded in Coppeliasim) is as follows:
Can somebody kindly tell me how can I modify (change) the embedded code in a way that the robot follows the joints values received from Moveit (Ros)?
Thank you in advance and regards,
Erfan
I am trying to control Franka_Panda robot through Moveit.
I can receive all the Franka_Panda joint positions in Coppeliasim with the following child script:
Code: Select all
function sysCall_init()
-- Enable subscriber:
if simROS then
sim.addLog(sim.verbosity_scriptinfos,"ROS interface was found.")
sub=simROS.subscribe('/joint_states', 'sensor_msgs/JointState', 'joint_callback')
simROS.subscriberTreatUInt8ArrayAsString(sub)
else
sim.addLog(sim.verbosity_scripterrors,"ROS interface was not found. Cannot run.")
end
end
function joint_callback(msg)
print(msg.position[1])
end
The example script with FrankaEmikaPanda (embedded in Coppeliasim) is as follows:
Code: Select all
function sysCall_init()
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 movCallback(config,vel,accel,handles)
for i=1,#handles,1 do
if sim.getJointMode(handles[i])==sim.jointmode_force and sim.isDynamicallyEnabled(handles[i]) then
sim.setJointTargetPosition(handles[i],config[i])
else
sim.setJointPosition(handles[i],config[i])
end
end
end
function moveToConfig(handles,maxVel,maxAccel,maxJerk,targetConf)
local currentConf={}
for i=1,#handles,1 do
currentConf[i]=sim.getJointPosition(handles[i])
end
sim.moveToConfig(-1,currentConf,nil,nil,maxVel,maxAccel,maxJerk,targetConf,nil,movCallback,handles)
end
function coroutineMain()
local jointHandles={-1,-1,-1,-1,-1,-1,-1}
for i=1,7,1 do
jointHandles[i]=sim.getObjectHandle('Franka_joint'..i)
end
local vel=90
local accel=40
local jerk=80
local maxVel={vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180,vel*math.pi/180}
local maxAccel={accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180,accel*math.pi/180}
local maxJerk={jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180,jerk*math.pi/180}
local targetPos1={90*math.pi/180,90*math.pi/180,135*math.pi/180,-45*math.pi/180,90*math.pi/180,180*math.pi/180,0}
moveToConfig(jointHandles,maxVel,maxAccel,maxJerk,targetPos1)
local targetPos2={-90*math.pi/180,90*math.pi/180,135*math.pi/180,-45*math.pi/180,90*math.pi/180,180*math.pi/180,0}
moveToConfig(jointHandles,maxVel,maxAccel,maxJerk,targetPos2)
local targetPos3={0,0,0,-90*math.pi/180,0,90*math.pi/180,0}
moveToConfig(jointHandles,maxVel,maxAccel,maxJerk,targetPos3)
end
Thank you in advance and regards,
Erfan