Synchronous mode with remote API Python

Typically: "How do I... ", "How can I... " questions
Post Reply
mtb_vrep
Posts: 1
Joined: 01 Jul 2018, 00:51

Synchronous mode with remote API Python

Post by mtb_vrep » 03 Oct 2018, 01:55

Hi, I am trying to send torques values to a jaco arm to be able to simulate it with torque control in synchronous mode but the arm starts responding a bit before falling to the ground.

First I made a pick&place simulation using the usual VREP tutorial about motion planning and recorded the torque values of each joint at a rate of 18 Hz and also the timestep of the recorded torques values with ROS.

So my goal is to be able to repeat the same motions made by the arm during the pick&place simulation with just torque values.
So I started a new scene https://drive.google.com/file/d/1rZ4XPN ... sp=sharing with the jaco arm and disabled the control position and just used the motor enabled option and set the target velocities to very high values.

On the client side I built a python script that triggered the next simulation timestep(dt=0.05). After sending torque and velocity values to the server side I triggered the next simulation step a number of loop time where the loop time value is calculated by dividing the time it took to get to the next torques values during the pick and place simulation by the simulation time dt. So during that period of waiting time the robot keeps that configuration of applied torques. Moreover when sending the torques I just send the absolute values of the torque and if the joint's torque is negative I set the velocity value to negative.

After many trials the robot keeps falling to the ground. I would appreciate if I can get a bit of help.
Thanks you.

Code: Select all

import numpy as np
import vrep
import time
import os
from utils import Csv

filename='data/torques.csv'

#simulation timestep
dt = .05

csv_reader = Csv(filename).read()

data =np.array([map(float,val) for val in csv_reader.values()])

data_times= data[10:,0:]
secs = [data_times[0][i+1] -data_times[0][i] for i in range(390)]
nsecs = [data_times[1][i+1] -data_times[1][i] for i in range(390)]

#Interval of time for time simulation
timestep = np.array(secs) + np.array(nsecs) / 1e09

#Torques values for replay
torques = [data[:6,i] for i in range(390)]


#Velocity of each joint is set to 10000.0 to allow the joint to just use the torque force
velocity = np.ones(6) * 10000.0

#Change the sign of the velocity if we have negative torques
velocity = np.multiply(velocity,np.sign([data[:6,i] for i in range(390)]))

#The number of times we trigger the simulation step before trying the next torque values
loop = [int(t) +1 for t in (timestep/dt)]



# close any open connections
vrep.simxFinish(-1)

# Connect to the V-REP continuous server
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 500, 5)

try:
    if clientID != -1: # if we connected successfully
        print('Connected to remote API server')

        # --------------------- Setup the simulation

        vrep.simxSynchronous(clientID, True)

        #Jaco joint_names
        joint_names = ['Jaco_joint%d'%i for i in range(1,7)]

        # Get the handles for each joint
        joint_handles = [vrep.simxGetObjectHandle(
            clientID,
            name,
            vrep.simx_opmode_blocking)[1] for name in joint_names]


        vrep.simxSetFloatingParameter(
            clientID,
            vrep.sim_floatparam_simulation_time_step,
            dt,  # specify a simulation time step
            vrep.simx_opmode_oneshot)

        # start our simulation in lockstep with our code
        vrep.simxStartSimulation(
            clientID,
            vrep.simx_opmode_blocking)

        k = 0
        # For 300 torques values retrieved from the csv files
        while k<300:


            #Send the torques values
            for ii, joint_handle in enumerate(joint_handles):
                vrep.simxSetJointTargetVelocity(
                    clientID,
                    joint_handle,
                    velocity[k][ii], # target velocity
                    vrep.simx_opmode_blocking)

                # and now modulate the force
                vrep.simxSetJointForce(
                    clientID,
                    joint_handle,
                    np.abs(torques[k][ii]),  # force to apply
                    vrep.simx_opmode_blocking)


            # For a required number of time trigger the next simulation step
            for t in range(loop[k]):
                vrep.simxSynchronousTrigger(clientID)
                time.sleep(1.0)






    else:
        raise Exception('Failed connecting to remote API server')
finally:
    # stop the simulation
    vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)

    vrep.simxGetPingTime(clientID)

    # Now close the connection to V-REP:
    vrep.simxFinish(clientID)
    print('connection closed...')




coppelia
Site Admin
Posts: 6763
Joined: 14 Dec 2012, 00:25

Re: Synchronous mode with remote API Python

Post by coppelia » 04 Oct 2018, 13:15

Hello,

you can't simply feed forward the recorded torques, since you will always eventually come out of a stable situation. Why don't you use a simple position controller for all of your joints?

Cheers

Post Reply