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.
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[i+1] -data_times[i] for i in range(390)] nsecs = [data_times[i+1] -data_times[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) 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...')