def doJointForceControl(self, jointIndex, inputForce, handJoint=False):
# get the current joint force
if (handJoint):
jointHandle = self._handJointHandles[jointIndex]
else:
jointHandle = self._jointHandles[jointIndex]
res, curForce = vrep.simxGetJointForce(self._clientID, jointHandle,
vrep.simx_opmode_blocking)
if res != 0: raise Exception()
curVel = RC.getJointVelocity(jointHandle)
# To make the joint balance around the position
# if force has changed signs,
# we need to change the target velocity sign
joint_target_vel = 10000
if (np.sign(curForce) < 0):
joint_target_vel *= (-1)
#print('SWITCH FORCE', jointIndex, curForce, curVel)
res = self.setJointVelocity(jointIndex, joint_target_vel, handJoint)
if res != 0: raise Exception()
# and now modulate the force
res = self.setJointForce(jointIndex, inputForce,
handJoint) # force to apply
if res != 0: raise Exception()
return res
def doJointVelocityControl(self, jointIndex, inputVel, handJoint=False):
# get the current joint force
if (handJoint):
jointHandle = self._handJointHandles[jointIndex]
else:
jointHandle = self._jointHandles[jointIndex]
# if force has changed signs,
# we need to change the target velocity sign
joint_max_torque = 10000
res = self.setJointForce(jointIndex, joint_max_torque, handJoint)
if res != 0: raise Exception()
# and now modulate the force
res, curForce = vrep.simxGetJointForce(self._clientID, jointHandle,
vrep.simx_opmode_blocking)
if res != 0: raise Exception()
if (np.sign(curForce) < 0):
inputVel *= (-1)
res = self.setJointVelocity(jointIndex, inputVel, handJoint)
if res != 0: raise Exception()
return res
def _set_joint_effort(
self, U
): # set torque U = [u1, u2, u3, u4, u5, u6, u7] to 7 joints in V-REP
torque = [
vrep.simxGetJointForce(self.clientID, self.handles['joint'][i],
vrep.simx_opmode_blocking)[1]
for i in range(self.action_dims)
] # retrieve the current torque from 7 joints
if len(U) != self.action_dims:
raise Exception('the dimension of action is wrong.')
# Give the torque and targeted velocity to joints.
for i in range(self.action_dims):
if U[i] > self._max_torque: # limit the torque of each joint under max_torque
U[i] = self._max_torque
if np.sign(torque[i]) * np.sign(U[i]) < 0:
self.joint_target_velocities[i] *= -1
_ = vrep.simxSetJointTargetVelocity(
self.clientID, self.handles['joint'][i],
self.joint_target_velocities[i], vrep.simx_opmode_blocking)
# Just in case
if _ != 0:
print('set target velocity error %s' % _)
raise Exception('failed to set target velocity to joints.')
_ = vrep.simxSetJointForce(self.clientID, self.handles['joint'][i],
abs(U[i]), vrep.simx_opmode_blocking)
# Just in case
if _ != 0:
print('set torques error %s' % _)
raise Exception('failed to set torques to joints.')
def getJointForce(clientID, joint_handle):
ret, get_joint_val = vrep.simxGetJointForce(clientID, joint_handle,
blocking)
if ret != return_ok:
raise Exception('FAIL: GetJointForce failed')
return get_joint_val
def getTorque(self, joints_handle, operation_mode=vrep.simx_opmode_buffer):
joints_torque = []
for joint in joints_handle:
err_flag, torque = vrep.simxGetJointForce(self.clientID, joint,
operation_mode)
if err_flag == -1:
return []
joints_torque.append(torque)
return joints_torque
def get_torques_firsttime():
for i in range(12):
print('handler is')
print(int(angles_handler[i]))
trash, torque = vrep.simxJointGetForce(clientID,
int(angles_handler[i]),
vrep.simx_opmode_streaming)
res = vrep.simx_return_novalue_flag
while res != vrep.simx_return_ok:
res, torque1 = vrep.simxGetJointForce(clientID,
int(angles_handler[i]),
vrep.simx_opmode_buffer)
print("TORQUE x IS")
print(torque1)
def get_torque(self, first=False):
"""[Returns the torque exerted on the joint]
[Different physics engine will return different values]
Keyword Arguments:
first {bool} -- [If true, the function is called in streaming mode,
The first call to the function should always be as first = True] (default: {False})
Returns:
[float] -- [torque or force exerted on the joint]
"""
assert self.handle is not None, "{} Handle does not exist".format(
self.vrep_name)
mode = streaming if first else buffering
rc, torque = vrep.simxGetJointForce(clientID, self.handle, mode)
return round(torque, 3)
def get_torque(joint):
angles = []
error = []
if joint == 'ab3' or joint == 0:
trash, torque = vrep.simxGetJointForce(clientID,
int(angles_handler[0]),
vrep.simx_opmode_buffer)
# angels.append(ang)
elif joint == 'bc3' or joint == 1:
trash, torque = vrep.simxGetJointForce(clientID,
int(angles_handler[1]),
vrep.simx_opmode_buffer)
# angels.append(ang)
elif joint == 'cd3' or joint == 2:
trash, torque = vrep.simxGetJointForce(clientID,
int(angles_handler[2]),
vrep.simx_opmode_buffer)
# angels.append(ang)
elif joint == 'ab4' or joint == 3:
trash, torque = vrep.simxGetJointForce(clientID,
int(angles_handler[3]),
vrep.simx_opmode_buffer)
# angels.append(ang)
elif joint == 'bc4' or joint == 4:
trash, torque = vrep.simxGetJointForce(clientID,
int(angles_handler[4]),
vrep.simx_opmode_buffer)
# angels.append(ang)
elif joint == 'cd4' or joint == 5:
trash, torque = vrep.simxGetJointForce(clientID,
int(angles_handler[5]),
vrep.simx_opmode_buffer)
# angels.append(ang)
elif joint == 'ab1' or joint == 6:
trash, torque = vrep.simxGetJointForce(clientID,
int(angles_handler[6]),
vrep.simx_opmode_buffer)
# angels.append(ang)
elif joint == 'bc1' or joint == 7:
trash, torque = vrep.simxGetJointForce(clientID,
int(angles_handler[7]),
vrep.simx_opmode_buffer)
# angels.append(ang)
elif joint == 'cd1' or joint == 8:
trash, torque = vrep.simxGetJointForce(clientID,
int(angles_handler[8]),
vrep.simx_opmode_buffer)
# angels.append(ang)
elif joint == 'ab2' or joint == 9:
trash, torque = vrep.simxGetJointForce(clientID,
int(angles_handler[9]),
vrep.simx_opmode_buffer)
# angels.append(ang)
elif joint == 'bc2' or joint == 10:
trash, torque = vrep.simxGetJointForce(clientID,
int(angles_handler[10]),
vrep.simx_opmode_buffer)
# angels.append(ang)
elif joint == 'cd2' or joint == 11:
trash, torque = vrep.simxGetJointForce(clientID,
int(angles_handler[11]),
vrep.simx_opmode_buffer)
# angels.append(ang)
return torque
def read_force(self, handle, mode='blocking'):
return vrep.simxGetJointForce(self.client_id, handle, self.modes[mode])
def doInverseKinematicsCalculation(self):
count = 0
target_index = 0
change_target_time = dt * 1000
vmax = 0.5
kp = 200.0
kv = np.sqrt(kp)
# define variables to share with nengo
q = np.zeros(len(joint_handles))
dq = np.zeros(len(joint_handles))
# NOTE: main loop starts here -----------------------------------------
target_xyz = RC.getObjectWorldPosition("obstacle")
track_target.append(np.copy(target_xyz)) # store for plotting
target_xyz = np.asarray(target_xyz)
for ii, joint_handle in enumerate(joint_handles):
old_q = np.copy(q)
# get the joint angles
_, q[ii] = vrep.simxGetJointPosition(clientID, joint_handle,
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception()
# get the joint velocity
_, dq[ii] = vrep.simxGetObjectFloatParameter(
clientID,
joint_handle,
2012, # parameter ID for angular velocity of the joint
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception()
# calculate position of the end-effector
# derived in the ur5 calc_TnJ class
xyz = robot_config.Tx('EE', q)
# calculate the Jacobian for the end effector
JEE = robot_config.J('EE', q)
# calculate the inertia matrix in joint space
Mq = robot_config.Mq(q)
# calculate the effect of gravity in joint space
Mq_g = robot_config.Mq_g(q)
# convert the mass compensation into end effector space
Mx_inv = np.dot(JEE, np.dot(np.linalg.inv(Mq), JEE.T))
svd_u, svd_s, svd_v = np.linalg.svd(Mx_inv)
# cut off any singular values that could cause control problems
singularity_thresh = .00025
for i in range(len(svd_s)):
svd_s[i] = 0 if svd_s[i] < singularity_thresh else \
1./float(svd_s[i])
# numpy returns U,S,V.T, so have to transpose both here
Mx = np.dot(svd_v.T, np.dot(np.diag(svd_s), svd_u.T))
# calculate desired force in (x,y,z) space
dx = np.dot(JEE, dq)
# implement velocity limiting
lamb = kp / kv
x_tilde = xyz - target_xyz
sat = vmax / (lamb * np.abs(x_tilde))
scale = np.ones(3)
if np.any(sat < 1):
index = np.argmin(sat)
unclipped = kp * x_tilde[index]
clipped = kv * vmax * np.sign(x_tilde[index])
scale = np.ones(3) * clipped / unclipped
scale[index] = 1
u_xyz = -kv * (dx -
np.clip(sat / scale, 0, 1) * -lamb * scale * x_tilde)
u_xyz = np.dot(Mx, u_xyz)
# transform into joint space, add vel and gravity compensation
u = np.dot(JEE.T, u_xyz) - Mq_g
# calculate the null space filter
Jdyn_inv = np.dot(Mx, np.dot(JEE, np.linalg.inv(Mq)))
null_filter = (np.eye(robot_config.num_joints) -
np.dot(JEE.T, Jdyn_inv))
# calculate our secondary control signal
q_des = np.zeros(robot_config.num_joints)
dq_des = np.zeros(robot_config.num_joints)
# calculated desired joint angle acceleration using rest angles
for ii in range(1, robot_config.num_joints):
if robot_config.rest_angles[ii] is not None:
q_des[ii] = (((robot_config.rest_angles[ii] - q[ii]) + np.pi) %
(np.pi * 2) - np.pi)
dq_des[ii] = dq[ii]
# only compensate for velocity for joints with a control signal
nkp = kp * .1
nkv = np.sqrt(nkp)
u_null = np.dot(Mq, (nkp * q_des - nkv * dq_des))
u += np.dot(null_filter, u_null)
# get the (x,y,z) position of the center of the obstacle
v = RC.getObjectWorldPosition('obstacle')
v = np.asarray(v)
# find the closest point of each link to the obstacle
for ii in range(robot_config.num_joints):
# get the start and end-points of the arm segment
p1 = robot_config.Tx('joint%i' % ii, q=q)
if ii == robot_config.num_joints - 1:
p2 = robot_config.Tx('EE', q=q)
else:
p2 = robot_config.Tx('joint%i' % (ii + 1), q=q)
# calculate minimum distance from arm segment to obstacle
# the vector of our line
vec_line = p2 - p1
# the vector from the obstacle to the first line point
vec_ob_line = v - p1
# calculate the projection normalized by length of arm segment
projection = np.dot(vec_ob_line, vec_line) / np.sum((vec_line)**2)
if projection < 0:
# then closest point is the start of the segment
closest = p1
elif projection > 1:
# then closest point is the end of the segment
closest = p2
else:
closest = p1 + projection * vec_line
# calculate distance from obstacle vertex to the closest point
dist = np.sqrt(np.sum((v - closest)**2))
# account for size of obstacle
rho = dist - obstacle_radius
if rho < threshold:
eta = .02 # feel like i saw 4 somewhere in the paper
drhodx = (v - closest) / rho
Fpsp = (eta * (1.0 / rho - 1.0 / threshold) * 1.0 / rho**2 *
drhodx)
# get offset of closest point from link's reference frame
T_inv = robot_config.T_inv('link%i' % ii, q=q)
m = np.dot(T_inv, np.hstack([closest, [1]]))[:-1]
# calculate the Jacobian for this point
Jpsp = robot_config.J('link%i' % ii, x=m, q=q)[:3]
# calculate the inertia matrix for the
# point subjected to the potential space
Mxpsp_inv = np.dot(Jpsp, np.dot(np.linalg.pinv(Mq), Jpsp.T))
svd_u, svd_s, svd_v = np.linalg.svd(Mxpsp_inv)
# cut off singular values that could cause problems
singularity_thresh = .00025
for ii in range(len(svd_s)):
svd_s[ii] = 0 if svd_s[ii] < singularity_thresh else \
1./float(svd_s[ii])
# numpy returns U,S,V.T, so have to transpose both here
Mxpsp = np.dot(svd_v.T, np.dot(np.diag(svd_s), svd_u.T))
u_psp = -np.dot(Jpsp.T, np.dot(Mxpsp, Fpsp))
if rho < .01:
u = u_psp
else:
u += u_psp
# multiply by -1 because torque is opposite of expected
u *= -1
print('u: ', u)
for ii, joint_handle in enumerate(joint_handles):
# the way we're going to do force control is by setting
# the target velocities of each joint super high and then
# controlling the max torque allowed (yeah, i know)
# get the current joint torque
_, torque = \
vrep.simxGetJointForce(
clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception()
# if force has changed signs,
# we need to change the target velocity sign
if np.sign(torque) * np.sign(u[ii]) <= 0:
joint_target_velocities[ii] = \
joint_target_velocities[ii] * -1
_ = self.setJointVelocity(
ii, joint_target_velocities[ii]) # target velocity
if _ != 0:
raise Exception()
# and now modulate the force
_ = self.setJointForce(ii, abs(u[ii])) # force to apply
if _ != 0:
raise Exception()
def obj_get_joint_force(self, handle): force = self.RAPI_rc(vrep.simxGetJointForce( self.cID,handle, self.opM_get)) return force
def vrepControl():
dataForce = 0
### defining in/out address/mode
address_rx = (ss_embsys_app_ip, kin_link_3)
address_tx = (ss_com_ip, hap_link_0)
mode_rx = "udp"
mode_tx = "udp"
### initialization
obj_rx = transfers.init_rx(address_rx, mode_rx)
obj_tx = transfers.init_tx(address_tx, mode_tx)
while (1):
print "vrepControl", time.time()
### receive message
msg = transfers.receive(obj_rx)
ping.echo_back(msg, "tpf_ss_embsys_entry") #ss_embsys tp's
### decode message
msg_list = codec.generic.decode(msg)
print msg_list
### moving actuators
msg_list = map(int, msg_list)
print "received", msg_list
jointAngle1 = msg_list[0]
jointAngle2 = msg_list[1]
jointAngle3 = msg_list[2]
jointAngle4 = msg_list[3]
gripperPosition = msg_list[4] #0-100, Full Close=0, Full Open = 100
errorcode = vrep.simxSetJointTargetPosition(clientID, jointHandle_1,
jointAngle1 * 3.14 / 180,
vrep.simx_opmode_streaming)
errorcode = vrep.simxSetJointTargetPosition(clientID, jointHandle_2,
jointAngle2 * 3.14 / 180,
vrep.simx_opmode_streaming)
errorcode = vrep.simxSetJointTargetPosition(clientID, jointHandle_3,
jointAngle3 * 3.14 / 180,
vrep.simx_opmode_streaming)
errorcode = vrep.simxSetJointTargetPosition(clientID, jointHandle_4,
jointAngle4 * 3.14 / 180,
vrep.simx_opmode_streaming)
errorcode = vrep.simxSetIntegerSignal(clientID,
'PhantomXPincher_gripperClose',
gripperPosition,
vrep.simx_opmode_oneshot)
### reading haptic data
[errorcode,
dataForce] = vrep.simxGetJointForce(clientID, jointHandle_4,
vrep.simx_opmode_streaming)
dataForce = -1 * dataForce
### Restricting pressure between 0 and 100
if (dataForce < 0):
dataForce = 0
if (dataForce > 100):
dataForce = 100
dataForce = int(100 * dataForce)
print "force", dataForce
### coding haptic data
msg_list = [dataForce]
msg = codec.generic.codev2(msg, msg_list)
### send the message
transfers.send(obj_tx, msg)
ping.echo_back(msg, "tpf_ss_embsys_exit")
transfers.close(obj_tx)
transfers.close(obj_rx)
return
def get_force(self):
code, force = v.simxGetJointForce(self._id, self._handle,
self._def_op_mode)
return force
def controller_motor(clientID, target_handle, self_handle, joint_handles,
firstPass):
# global firstPass
global joint_target_velocities
global motor_mask
global u
if (firstPass):
joint_target_velocities = np.ones(len(joint_handles)) * 10000.0
u = [1] * len(joint_handles)
motor_mask = [0] * len(joint_handles)
print("OMG FIRSTPASS")
#-- Decide of the motor velocities:
#Grab target
#error, target_pos = vrep.simxGetObjectPosition(cid,target_handle,self_handle,mode())
#Grab joint angles
q = np.zeros(len(joint_handles))
dq = np.zeros(len(joint_handles))
for ii, joint_handle in enumerate(joint_handles):
if (ii < 2):
continue
# get the joint angles
_, q[ii] = vrep.simxGetJointPosition(clientID, joint_handle,
vrep.simx_opmode_oneshot_wait)
if _ != 0: raise Exception()
# get the joint velocity
_, dq[ii] = vrep.simxGetObjectFloatParameter(
clientID,
joint_handle,
2012, # ID for angular velocity of the joint
vrep.simx_opmode_oneshot_wait)
if _ != 0: raise Exception()
# get the current joint torque
_, torque = vrep.simxGetJointForce(clientID, joint_handle,
vrep.simx_opmode_oneshot_wait)
if _ != 0: raise Exception()
joint_target_velocities[ii] = 5
u[ii] = np.random.uniform(-500, 500)
motor_mask[2] = 1
# if force has changed signs,
# we need to change the target velocity sign
vrep.simxPauseCommunication(clientID, 1)
for ii, joint_handle in enumerate(joint_handles):
if np.sign(torque) * np.sign(u[ii]) < 0:
joint_target_velocities[ii] = joint_target_velocities[ii] * -1
if (motor_mask[ii]):
vrep.simxSetJointTargetVelocity(clientID, joint_handle,
joint_target_velocities[ii],
vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(
clientID,
joint_handle,
abs(u[ii]), # force to apply
vrep.simx_opmode_oneshot)
if _ != 0: raise Exception()
vrep.simxPauseCommunication(clientID, 0)
firstPass = False
return firstPass
# calculate the effects of gravity
Mq_g = Mm12_joint_g
u = -Mq_g
u *= -1 # because the joints on the arm are backwards
print 'u: ', u
for ii,joint_handle in enumerate(joint_handles):
# the way we're going to do force control is by setting
# the target velocities of each joint super high and then
# controlling the max torque allowed (yeah, i know)
# get the current joint torque
_, torque = \
vrep.simxGetJointForce(clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# if force has changed signs,
# we need to change the target velocity sign
if np.sign(torque) * np.sign(u[ii]) <= 0:
joint_target_velocities[ii] = \
joint_target_velocities[ii] * -1
vrep.simxSetJointTargetVelocity(clientID,
joint_handle,
joint_target_velocities[ii], # target velocity
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
# and now modulate the force
def getTrajectories():
print ('Program started')
#Start Grabbing Handles
functions = {}
args = {}
real_fun(functions)
#1-15 are examples along a sphere, one at top of sphere, one in middle, one in bottom,
#and then three rings at centred .25d, .5d, .75d with radius = 2
k = functions.keys()
k.sort()
iter = 1
for key in k:
firstPass = True
state_file = open('trajectories/target/{}.txt'.format(key+"state"),'w+')
action_file = open('trajectories/target/{}.txt'.format(key+"action"),'w+')
vrep.simxFinish(-1) # just in case, close all opened connections
time.sleep(1)
cid = connect()
time.sleep(1)
vrep.simxLoadScene(cid,'scene/arm_2.ttt',0,mode())
dt = .01
vrep.simxSetFloatingParameter(cid,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
vrep.simxStartSimulation(cid,vrep.simx_opmode_streaming)
runtime = 0
joint_names = ['redundantRob_joint'+str(x) for x in range(1,8)]
# print(joint_names)
# # joint target velocities discussed below
# joint_target_velocities = np.ones(len(joint_names)) * 10000.0
#
# # get the handles for each joint and set up streaming
joint_handles = [vrep.simxGetObjectHandle(cid,
name, vrep.simx_opmode_oneshot_wait)[1] for name in joint_names]
# print(joint_handles)
# # get handle for target and set up streaming
_, target_handle = vrep.simxGetObjectHandle(cid,
'redundantRob_manipSphere', vrep.simx_opmode_oneshot_wait)
#
_, self_handle = vrep.simxGetObjectHandle(cid,'Rectangle',vrep.simx_opmode_oneshot_wait)
#
_, target_pos = vrep.simxGetObjectPosition(cid,
target_handle, -1, vrep.simx_opmode_oneshot_wait)
#This prepared the data
real_args(args, target_pos)
print(key)
while(runtime < 0.4):
target_move(cid,target_handle, firstPass, functions[key], args[key])
#vrep.simxSynchronous(cid, False)
vrep.simxGetPingTime(cid)
firstPass = controller_motor(cid,target_handle,self_handle, joint_handles, firstPass)
commands = [vrep.simxGetJointForce(cid, joint, vrep.simx_opmode_oneshot_wait)[1] for joint in
joint_handles]
joint_pos = [vrep.simxGetObjectPosition(cid, joint, vrep.sim_handle_parent, vrep.simx_opmode_oneshot_wait)[1] for joint
in joint_handles]
joint_vel = [vrep.simxGetObjectFloatParameter(cid, joint, 2012, vrep.simx_opmode_blocking)[1]
for joint in joint_handles]
target_rel = vrep.simxGetObjectPosition(cid, target_handle, joint_handles[-1],
vrep.simx_opmode_oneshot_wait)[1]
joint_state = np.append(np.asarray(joint_pos).flatten(),target_rel + joint_vel)
# joint_state = np.append(np.asarray(joint_pos).flatten(),joint_vel)
# print(np.asarray(joint_state).shape)
# print(joint_state)
state_file.write("{}\n".format(",".join(
[str(x) for x in joint_state])))
action_file.write(
"{}\n".format(" ".join([str(x)[1:-2] for x in commands])))
runtime += dt
vrep.simxSynchronousTrigger(cid)
firstPass = False
vrep.simxStopSimulation(cid, vrep.simx_opmode_streaming)
vrep.simxSynchronousTrigger(cid)
vrep.simxFinish(cid)
firstPass = True
print("meow")
import vrep
import sys
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19999, True, True, 5000, 5)
if clientID != -1:
print('Connected to remove API server')
else:
print("Connection not successful")
sys.exit('Error: Could not connect')
errorCode, joint_2 = vrep.simxGetObjectHandle(clientID, 'redundantRob_joint2',
vrep.simx_opmode_oneshot_wait)
def joint_2_force():
vrep.simxSetJointForce(clientID, joint_2, 0.01, vrep.simx_opmode_oneshot)
while True:
joint_2_force()
returnCode, force = vrep.simxGetJointForce(clientID, joint_2,
vrep.simx_opmode_streaming)
print(force)
def send_forces(self, u):
""" Apply the specified torque to the robot joints
Apply the specified torque to the robot joints, move the simulation
one time step forward, and update the position of the hand object.
Parameters
----------
u : np.array
the torques to apply to the robot
"""
# invert because VREP torque directions are opposite of expected
u *= -1
for ii, joint_handle in enumerate(self.joint_handles):
# get the current joint torque
_, torque = vrep.simxGetJointForce(
self.clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception('Error retrieving joint torque, ' +
'return code ', _)
# if force has changed signs,
# we need to change the target velocity sign
if np.sign(torque) * np.sign(u[ii]) <= 0:
self.joint_target_velocities[ii] *= -1
_ = vrep.simxSetJointTargetVelocity(
self.clientID,
joint_handle,
self.joint_target_velocities[ii],
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception('Error setting joint target velocity, ' +
'return code ', _)
# and now modulate the force
_ = vrep.simxSetJointForce(self.clientID,
joint_handle,
abs(u[ii]), # force to apply
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception('Error setting max joint force, ' +
'return code ', _)
# Update position of hand object
hand_xyz = self.robot_config.Tx(name='EE', q=self.q)
self.set_xyz('hand', hand_xyz)
# Update orientation of hand object
quaternion = self.robot_config.orientation('EE', q=self.q)
angles = transformations.euler_from_quaternion(
quaternion, axes='rxyz')
self.set_orientation('hand', angles)
# move simulation ahead one time step
vrep.simxSynchronousTrigger(self.clientID)
self.count += self.dt
def obj_get_joint_force(self, handle):
force = self.RAPI_rc(
vrep.simxGetJointForce(self.cID, handle,
vrep.simx_opmode_blocking))
return force
# vrep.simxSetObjectPosition(clientID, mass_handles[2], -1, (x3-0.1*(sim_time-2), y3, z3), vrep.simx_opmode_blocking)
# Set the motor forces to zero - let them freewheel?
for motor in motor_handles:
vrep.simxSetJointForce(clientID, motor, 1e-3,
vrep.simx_opmode_blocking)
else:
#Set the motor forces to zero - let them freewheel?
for motor in motor_handles:
vrep.simxSetJointForce(clientID, motor, 0.01,
vrep.simx_opmode_blocking)
# vrep.simxSetJointTargetVelocity(clientID, motor, 0, vrep.simx_opmode_blocking)
# Get the spring/damper forces
_, force = vrep.simxGetJointForce(clientID, joint_handles[0],
vrep.simx_opmode_blocking)
print('Time: {:0.2f} \t Spring Force: {:0.2f}'.format(
sim_time, force))
# Get the position of the boddy at each timestep and print it out,
# -1 in this call means absolute position
_, (x, y,
z) = vrep.simxGetObjectPosition(clientID, mass_handles[0], -1,
vrep.simx_opmode_blocking)
print('Mass 1 -- x: {:5.2f} \t y: {:5.2f} \t z: {:5.2f}'.format(
x, y, z))
_, (x, y,
z) = vrep.simxGetObjectPosition(clientID, mass_handles[1], -1,
vrep.simx_opmode_blocking)
print(
# get handle on component to move, i.e. j1=simGetObjectHandle("JacoHand_fingers12_motor2")
# if object does not exist, error code 8 and handle of 0
## gets handle of jaco hand joints
errorCode, motor1handle = vrep.simxGetObjectHandle(clientID, "JacoHand_fingers12_motor1", vrep.simx_opmode_oneshot_wait)
errorCode, motor2handle = vrep.simxGetObjectHandle(clientID, "JacoHand_fingers12_motor2", vrep.simx_opmode_oneshot_wait)
## gets handle of jaco arm joints, one at a time
errorCode, joint1handle = vrep.simxGetObjectHandle(clientID, "Jaco_joint1", vrep.simx_opmode_oneshot_wait)
errorCode, joint2handle = vrep.simxGetObjectHandle(clientID, "Jaco_joint2",
vrep.simx_opmode_oneshot_wait)
print "getting joint information"
## gets force and position of joints
errorCode1, initPosition1 = vrep.simxGetJointPosition(clientID, joint1handle, vrep.simx_opmode_oneshot_wait)
errorCode1, initForce1 = vrep.simxGetJointForce(clientID, joint1handle, vrep.simx_opmode_oneshot_wait)
errorCode2, initPosition2 = vrep.simxGetJointPosition(clientID, joint2handle, vrep.simx_opmode_oneshot_wait)
errorCode2, initForce2 = vrep.simxGetJointForce(clientID, joint2handle, vrep.simx_opmode_oneshot_wait)
errorCode1, initPosition4 = vrep.simxGetJointPosition(clientID, motor1handle, vrep.simx_opmode_oneshot_wait)
print "set joint information"
## pause stream and prepare dataload to send
vrep.simxPauseCommunication(clientID, True)
errorCode2 = vrep.simxSetJointTargetVelocity(clientID, joint1handle, 0.2, vrep.simx_opmode_oneshot)
errorCode2 = vrep.simxSetJointForce(clientID, joint1handle, 15, vrep.simx_opmode_oneshot)
errorCode2 = vrep.simxSetJointTargetVelocity(clientID, joint2handle, 0.2, vrep.simx_opmode_oneshot)
errorCode2 = vrep.simxSetJointForce(clientID, joint2handle, 15, vrep.simx_opmode_oneshot)
## send data stream
vrep.simxPauseCommunication(clientID, False)
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# strt 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 < 700:
#Send the torques values
uu = torques[k]
for ii, joint_handle in enumerate(joint_handles):
_, u = vrep.simxGetJointForce(clientID, joint_handle,
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception(
'Error retrieving joint torque, ' + 'return code ', _)
# if np.sign(u) * np.sign(uu[ii]) <= 0:
#
# velocity[ii] *= -1
#
# _ = vrep.simxSetJointTargetVelocity(
# clientID,
# joint_handle,
# velocity[ii],
# vrep.simx_opmode_blocking)
# if _ != 0:
# raise Exception('Error setting joint target velocity, ' +
def callback(msg):
global count
global dt
global _
if clientID != -1 and count == 0: # if we connected successfully
print('Connected to remote API server')
# --------------------- Setup the simulation
vrep.simxSynchronous(clientID, True)
global joint_names
# joint target velocities discussed below
global joint_target_velocities
joint_target_velocities = np.ones(len(joint_names)) * 10000.0
global joint_handles
# get the handles for each joint and set up streaming
joint_handles = [
vrep.simxGetObjectHandle(clientID, name,
vrep.simx_opmode_blocking)[1]
for name in joint_names
]
# get handle for target and set up streaming
global target_handle
_, target_handle = vrep.simxGetObjectHandle(clientID, 'target',
vrep.simx_opmode_blocking)
vrep.simxSetFloatingParameter(
clientID,
vrep.sim_floatparam_simulation_time_step,
dt, # specify a simulation time step
vrep.simx_opmode_oneshot)
# --------------------- Start the simulation
# start our simulation in lockstep with our code
vrep.simxStartSimulation(clientID, vrep.simx_opmode_blocking)
print("Starting Simulation")
if clientID != -1 and count >= 0 and count < 6: # run for 1 simulated second
target_xyz = [msg.x, msg.y, msg.z]
# store for plotting
global track_target
track_target.append(np.copy(target_xyz))
# get the (x,y,z) position of the target
vrep.simxSetObjectPosition(clientID, target_handle, -1, target_xyz,
vrep.simx_opmode_blocking)
if _ != 0: raise Exception("Deu ruim parceiro")
target_xyz = np.asarray(target_xyz)
q = np.zeros(len(joint_handles))
dq = np.zeros(len(joint_handles))
for ii, joint_handle in enumerate(joint_handles):
# get the joint angles
_, q[ii] = vrep.simxGetJointPosition(clientID, joint_handle,
vrep.simx_opmode_blocking)
if _ != 0: raise Exception()
# get the joint velocity
_, dq[ii] = vrep.simxGetObjectFloatParameter(
clientID,
joint_handle,
2012, # parameter ID for angular velocity of the joint
vrep.simx_opmode_blocking)
if _ != 0: raise Exception()
L = np.array([0, .42, .225]) # arm segment lengths
xyz = np.array([L[0]*np.cos(q[0])+L[1]*np.cos(q[0])*np.cos(q[1])+L[2]*np.cos(q[0])*np.cos(q[1])*np.cos(q[2])- \
L[2]*np.cos(q[0])*np.sin(q[1])*np.sin(q[2]),
L[0]*np.cos(q[0])+L[1]*np.cos(q[1])*np.sin(q[0])+L[2]*np.sin(q[0])*np.cos(q[1])*np.cos(q[2])- \
L[2]*np.sin(q[0])*np.sin(q[1])*np.sin(q[2]),
# have to add .1 offset to z position
L[1]*np.sin(q[1])+L[2]*(np.cos(q[1])*np.sin(q[2])+np.cos(q[2])*np.sin(q[1]))+.14])
global track_hand
track_hand.append(np.copy(xyz)) #store for plotting
# calculate the Jacobian for the hand
JEE = np.zeros((3, 3))
JEE[0, 2] = -L[2] * np.sin(q[1] + q[2]) * np.cos(q[0])
JEE[1, 2] = -L[2] * np.sin(q[1] + q[2]) * np.sin(q[0])
JEE[2, 2] = L[2] * np.cos(q[1] + q[2])
JEE[0, 1] = -np.cos(q[0]) * (L[2] * np.sin(q[1] + q[2]) +
L[1] * np.sin(q[1]))
JEE[1, 1] = -np.sin(q[0]) * (L[2] * np.sin(q[1] + q[2]) +
L[1] * np.sin(q[1]))
JEE[2, 1] = L[2] * np.cos(q[1] + q[2]) + L[1] * np.cos(q[1])
JEE[0, 0] = -np.sin(q[0]) * (L[0] + JEE[2][1])
JEE[1, 0] = np.cos(q[0]) * (L[0] + JEE[2][1])
# get the Jacobians for the centres-of-mass for the arm segments
JCOM1 = np.zeros((6, 3))
JCOM1[0, 0] = L[0] * -np.sin(q[0]) / 2
JCOM1[1, 0] = L[0] * np.cos(q[0]) / 2
JCOM1[5, 0] = 1.0
JCOM2 = np.zeros((6, 3))
JCOM2[0, 1] = L[1] * (np.cos(q[1]) * np.sin(q[0]) -
np.cos(q[0]) * np.sin(q[1])) / 2
JCOM2[1, 1] = L[1] * (-np.cos(q[1]) * np.cos(q[0]) -
np.sin(q[1]) * np.sin(q[0])) / 2
JCOM2[4, 1] = 1.0
JCOM2[0, 0] = L[1] * (np.cos(q[0]) * np.sin(q[1]) - np.cos(q[1]) *
np.sin(q[0])) / 2 - L[0] * np.sin(q[0])
JCOM2[1,
0] = L[1] * (np.sin(q[0]) * np.sin(q[1]) +
np.cos(q[0]) * np.cos(q[1])) / 2 + 2 * JCOM1[1][0]
JCOM2[4, 0] = 1.0
JCOM3 = np.zeros((6, 3))
JCOM3[0, 2] = L[2] * (np.cos(q[2]) * np.sin(q[0]) -
np.cos(q[0]) * np.cos(q[1]) * np.sin(q[2])) / 2
JCOM3[1, 2] = L[2] * (-np.cos(q[2]) * np.cos(q[0]) -
np.cos(q[1]) * np.sin(q[0]) * np.sin(q[2])) / 2
JCOM3[2, 2] = -L[2] * np.sin(q[1]) * np.sin(q[2]) / 2
JCOM3[4, 2] = 1.0
JCOM3[0, 1] = L[2] * -np.cos(q[0]) * np.cos(q[2]) * np.sin(
q[1]) / 2 - L[1] * np.cos(q[0]) * np.sin(q[1])
JCOM3[1, 1] = L[2] * -np.cos(q[2]) * np.sin(q[0]) * np.sin(
q[1]) / 2 - L[1] * np.sin(q[0]) * np.sin(q[1])
JCOM3[2, 1] = (L[2] * np.cos(q[1]) * np.cos(q[2]) /
2) + L[1] * np.cos(q[1])
JCOM3[4, 1] = 1.0
JCOM3[0,0] = L[2]*(np.cos(q[0])*np.sin(q[2]) - np.cos(q[1])*np.cos(q[2])*np.sin(q[0]))/2 -L[1]*np.cos(q[1])*np.sin(q[0])- \
L[0]*np.sin(q[0])
JCOM3[1,0] = L[2]*(np.sin(q[0])*np.sin(q[2]) + np.cos(q[0])*np.cos(q[1])*np.cos(q[2]))/2 + \
L[1]*np.cos(q[0])*np.cos(q[1])+2*JCOM1[1][0]
JCOM3[4, 0] = 1.0
m1 = 0.15 #from VREP
M1 = np.diag([m1, m1, m1, .001, .001, .001])
m2 = 1.171 # from VREP
M2 = np.diag([m1, m1, m1, .008, .008, .001])
m3 = .329 # from VREP
M3 = np.diag([m2, m2, m2, 5e-4, 5e-4, 1e-4])
# generate the mass matrix in joint space
Mq = np.dot(JCOM1.T, np.dot(M1, JCOM1)) + \
np.dot(JCOM2.T, np.dot(M2, JCOM2)) + \
np.dot(JCOM3.T, np.dot(M3, JCOM3))
# compensate for gravity
gravity = np.array([
0,
0,
-9.81,
0,
0,
0,
])
Mq_g = np.dot(JCOM1.T, np.dot(M1, gravity)) + \
np.dot(JCOM2.T, np.dot(M2, gravity)) + \
np.dot(JCOM2.T, np.dot(M2, gravity))
Mx_inv = np.dot(JEE, np.dot(np.linalg.inv(Mq), JEE.T))
Mu, Ms, Mv = np.linalg.svd(Mx_inv)
# cut off any np.singular values that could cause control problems
for i in range(len(Ms)):
Ms[i] = 0 if Ms[i] < 1e-5 else 1. / float(Ms[i])
# numpy returns U,S,V.T, so have to transpose both here
Mx = np.dot(Mv.T, np.dot(np.diag(Ms), Mu.T))
# calculate desired movement in operational (hand) space
kp = 100
kv = np.sqrt(kp)
u_xyz = np.dot(Mx, kp * (target_xyz - xyz))
u = np.dot(JEE.T, u_xyz) - np.dot(Mq, kv * dq) - Mq_g
u *= -1 # because the joints on the arm are backwards
for ii, joint_handle in enumerate(joint_handles):
# the way we're going to do force control is by setting
# the target velocities of each joint super high and then
# controlling the max torque allowed (yeah, i know)
# get the current joint torque
_, torque = \
vrep.simxGetJointForce(clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ != 0: raise Exception()
# if force has changed signs,
# we need to change the target velocity sign
if np.sign(torque) * np.sign(u[ii]) < 0:
joint_target_velocities[ii] = \
joint_target_velocities[ii] * -1
vrep.simxSetJointTargetVelocity(
clientID,
joint_handle,
joint_target_velocities[ii], # target velocity
vrep.simx_opmode_blocking)
if _ != 0: raise Exception()
# and now modulate the force
vrep.simxSetJointForce(
clientID,
joint_handle,
abs(u[ii]), # force to apply
vrep.simx_opmode_blocking)
if _ != 0: raise Exception()
# move simulation ahead one time step
vrep.simxSynchronousTrigger(clientID)
count += dt
print("Cheguei aqui uma vez")
elif clientID != -1 and count >= 6:
vrep.simxStopSimulation(clientID, vrep.simx_opmode_blocking)
# Before closing the connection to V-REP,
#make sure that the last command sent out had time to arrive.
vrep.simxGetPingTime(clientID)
# Now close the connection to V-REP:
vrep.simxFinish(clientID)
else:
raise Exception('Failed connecting to remote API server')
return
