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