Moveit to CoppeliaSim

Typically: "How do I... ", "How can I... " questions
Post Reply
Erfan
Posts: 16
Joined: 23 Mar 2021, 02:26

Moveit to CoppeliaSim

Post by Erfan »

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:

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
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:

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
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

fferri
Posts: 1193
Joined: 09 Sep 2013, 19:28

Re: Moveit to CoppeliaSim

Post by fferri »

Code: Select all

function joint_callback(msg)
    movCallback(msg.position)
end
?

Post Reply