def getJointVelocity(self, handle, ignoreError = False, initialize = False):
if initialize:
sim.simxGetObjectFloatParameter(self.clientID, handle, 2012, sim.simx_opmode_streaming)
res,velocity=sim.simxGetObjectFloatParameter(self.clientID, handle, 2012, sim.simx_opmode_buffer)
if res!=sim.simx_return_ok and not ignoreError:
print('Failed to get joint velocity')
print(res)
return velocity
def get_vel_firsttime():
counter = 0
for i in range(12):
print('handler is')
print(int(angles_handler[i]))
trash, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[i]),
2012,
sim.simx_opmode_buffer)
res = sim.simx_return_novalue_flag
while res != sim.simx_return_ok:
res, vel = sim.simxGetObjectFloatParameter(
clientID, int(angles_handler[i]), 2012,
sim.simx_opmode_streaming)
print('counter is ' + str(counter) + "Velocity x IS")
counter = counter + 1
print(vel)
def update_motors(self):
"""Get new measurements for velocity and update displacement"""
# Collect joint params
_, wL = sim.simxGetObjectFloatParameter(self.client_id,
self.left_motor_handle, 2012,
sim.simx_opmode_oneshot_wait)
# sim.sim_jointfloatparam_velocity(self.left_motor_handle)
_, wR = sim.simxGetObjectFloatParameter(self.client_id,
self.right_motor_handle, 2012,
sim.simx_opmode_oneshot_wait)
# _, wR = sim.sim_jointfloatparam_velocity(self.right_motor_handle)
# Get time elapsed in seconds
delta_time = (datetime.now() - self.last_time).total_seconds()
# Set prior time to now
self.last_time = datetime.now()
# Calculate thetas
theta_l = wL * delta_time * self.r
theta_r = wR * delta_time * self.r
# Update odometers
self.left_odometer += theta_l
self.right_odometer += theta_r
# Distance Traveled
delta_s = (theta_l + theta_r) / 2
# Velocity
self.velocity = delta_s / delta_time
# Change in Rotation
delta_theta = (theta_r - theta_l) / (2 * self.b) # b = width of cart
self.pose[2] = self.pose[2] + delta_theta
self.pose[0] = self.pose[0] + delta_s * np.cos(self.pose[2])
self.pose[1] = self.pose[1] + delta_s * np.sin(self.pose[2])
def get_size(clientID, obstacle):
size = np.empty(6)
err, size[0] = vrep.simxGetObjectFloatParameter (clientID, obstacle, vrep.sim_objfloatparam_objbbox_min_x, vrep.simx_opmode_oneshot_wait)
err, size[1] = vrep.simxGetObjectFloatParameter (clientID, obstacle, vrep.sim_objfloatparam_objbbox_min_y, vrep.simx_opmode_oneshot_wait)
err, size[2] = vrep.simxGetObjectFloatParameter (clientID, obstacle, vrep.sim_objfloatparam_objbbox_min_z, vrep.simx_opmode_oneshot_wait)
err, size[3] = vrep.simxGetObjectFloatParameter (clientID, obstacle, vrep.sim_objfloatparam_objbbox_max_x, vrep.simx_opmode_oneshot_wait)
err, size[4] = vrep.simxGetObjectFloatParameter (clientID, obstacle, vrep.sim_objfloatparam_objbbox_max_y, vrep.simx_opmode_oneshot_wait)
err, size[5] = vrep.simxGetObjectFloatParameter (clientID, obstacle, vrep.sim_objfloatparam_objbbox_max_z, vrep.simx_opmode_oneshot_wait)
if err > 0:
print("ERROR, ERROR, can't retrieve size")
return size
def __init__(self, name, clientID, deskId, displacements, category):
self.name = name
self.clientID = clientID
self.parent = deskId
res, handle = sim.simxGetObjectHandle(clientID, name,
sim.simx_opmode_blocking)
if res:
sys.exit(1)
self.handle = handle
# category of the object
self.cate = category
# displacement w.r.t to the lower left corner
x_displacement, y_displacement, z_displacement = displacements
self.x_displacement = x_displacement
self.y_displacement = y_displacement
self.z_displacement = z_displacement
# object position relative to the table
res, pos = sim.simxGetObjectPosition(clientID, handle, self.parent,
sim.simx_opmode_blocking)
self.pos = np.array(pos) + np.array([
x_displacement, y_displacement, 0
]) # position w.r.t to the lower left corner
self.actual_pos = self.pos.copy()
# geting the min, max value w.r.t to self frame
res, xmin = sim.simxGetObjectFloatParameter(
self.clientID, self.handle, sim.sim_objfloatparam_objbbox_min_x,
sim.simx_opmode_blocking)
res, xmax = sim.simxGetObjectFloatParameter(
self.clientID, self.handle, sim.sim_objfloatparam_objbbox_max_x,
sim.simx_opmode_blocking)
res, ymin = sim.simxGetObjectFloatParameter(
self.clientID, self.handle, sim.sim_objfloatparam_objbbox_min_y,
sim.simx_opmode_blocking)
res, ymax = sim.simxGetObjectFloatParameter(
self.clientID, self.handle, sim.sim_objfloatparam_objbbox_max_y,
sim.simx_opmode_blocking)
res, zmin = sim.simxGetObjectFloatParameter(
self.clientID, self.handle, sim.sim_objfloatparam_objbbox_min_z,
sim.simx_opmode_blocking)
res, zmax = sim.simxGetObjectFloatParameter(
self.clientID, self.handle, sim.sim_objfloatparam_objbbox_max_z,
sim.simx_opmode_blocking)
self.boundary_points = [xmin, xmax, ymin, ymax, zmin, zmax]
# initialize the bounding box
self.get_bbox()
def __init__(self, clientID):
res, desk = sim.simxGetObjectHandle(clientID, 'table',
sim.simx_opmode_blocking)
self.clientID = clientID
self.handle = desk
# get the dimension
res, xmin = sim.simxGetObjectFloatParameter(
self.clientID, self.handle, sim.sim_objfloatparam_objbbox_min_x,
sim.simx_opmode_blocking)
res, xmax = sim.simxGetObjectFloatParameter(
self.clientID, self.handle, sim.sim_objfloatparam_objbbox_max_x,
sim.simx_opmode_blocking)
res, ymin = sim.simxGetObjectFloatParameter(
self.clientID, self.handle, sim.sim_objfloatparam_objbbox_min_y,
sim.simx_opmode_blocking)
res, ymax = sim.simxGetObjectFloatParameter(
self.clientID, self.handle, sim.sim_objfloatparam_objbbox_max_y,
sim.simx_opmode_blocking)
res, zmin = sim.simxGetObjectFloatParameter(
self.clientID, self.handle, sim.sim_objfloatparam_objbbox_min_z,
sim.simx_opmode_blocking)
res, zmax = sim.simxGetObjectFloatParameter(
self.clientID, self.handle, sim.sim_objfloatparam_objbbox_max_z,
sim.simx_opmode_blocking)
# calculate the dimension, relative to self frame
self.x_val = xmax - xmin
self.y_val = ymax - ymin
self.z_val = zmax
self.init_occupancy_grid()
# find the transformation from desk frame to the world
res, transform = sim.simxGetObjectPosition(clientID, desk, -1,
sim.simx_opmode_blocking)
res, orientation = sim.simxGetObjectOrientation(
clientID, desk, -1, sim.simx_opmode_blocking)
self.world_transform = util.matrix_transform(orientation,
np.array(transform))
def get_vel(getangles):
angle = getangles
angles = []
error = []
if angle == 'ab3' or angle == 0:
res, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[0]),
2012,
sim.simx_opmode_streaming)
# angels.append(ang)
elif angle == 'bc3' or angle == 1:
res, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[1]),
2012,
sim.simx_opmode_streaming)
# angels.append(ang)
elif angle == 'cd3' or angle == 2:
res, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[2]),
2012,
sim.simx_opmode_streaming)
# angels.append(ang)
elif angle == 'ab4' or angle == 3:
res, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[3]),
2012,
sim.simx_opmode_streaming)
# angels.append(ang)
elif angle == 'bc4' or angle == 4:
res, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[4]),
2012,
sim.simx_opmode_streaming)
# angels.append(ang)
elif angle == 'cd4' or angle == 5:
res, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[5]),
2012,
sim.simx_opmode_streaming)
# angels.append(ang)
elif angle == 'ab1' or angle == 6:
res, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[6]),
2012,
sim.simx_opmode_streaming)
# angels.append(ang)
elif angle == 'bc1' or angle == 7:
res, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[7]),
2012,
sim.simx_opmode_streaming)
# angels.append(ang)
elif angle == 'cd1' or angle == 8:
res, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[8]),
2012,
sim.simx_opmode_streaming)
# angels.append(ang)
elif angle == 'ab2' or angle == 9:
res, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[9]),
2012,
sim.simx_opmode_streaming)
# angels.append(ang)
elif angle == 'bc2' or angle == 10:
res, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[10]),
2012,
sim.simx_opmode_streaming)
# angels.append(ang)
elif angle == 'cd2' or angle == 11:
res, vel = sim.simxGetObjectFloatParameter(clientID,
int(angles_handler[11]),
2012,
sim.simx_opmode_streaming)
# angels.append(ang)
return vel
sim.simxSetJointTargetVelocity(clientID, leftMotor, 5.0,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(clientID, rightMotor, 5.0,
sim.simx_opmode_oneshot)
# Ground robot move forward and get the diameter
sim.simxSetJointTargetVelocity(clientID, leftMotor,
groundrobot_forwardspeed,
sim.simx_opmode_oneshot)
sim.simxSetJointTargetVelocity(clientID, rightMotor,
groundrobot_forwardspeed,
sim.simx_opmode_oneshot)
leftWheelDiameter = \
sim.simxGetObjectFloatParameter(clientID, leftWheel, 18, sim.simx_opmode_oneshot)[1] \
- sim.simxGetObjectFloatParameter(clientID,
leftWheel, 15, sim.simx_opmode_oneshot)[1]
while (sim.simxGetConnectionId(clientID) != -1):
# Get image from Camera
res, resolution, image = sim.simxGetVisionSensorImage(
clientID, camera, 0, sim.simx_opmode_buffer)
# unique_values, delta = main_fun.currentVisionState(
# 'FINDING_TEDDY', res, resolution, image_gnd)
tshirt_x = IMG_WIDTH / 2
# Function of wheel odometery
leftWheelPosition = sim.simxGetJointPosition(
clientID, leftMotor, sim.simx_opmode_oneshot)[1]
dTheta = leftWheelPosition - lastLeftWheelPosition
index = 0
# 开始仿真
while vrep.simxGetConnectionId(clientID) != -1:
currCmdTime=vrep.simxGetLastCmdTime(clientID) # 记录当前时间
dt = currCmdTime - lastCmdTime # 记录时间间隔，用于控制
# ***
_, base_pos = vrep.simxGetObjectPosition(clientID, baseHandle, -1, vrep.simx_opmode_streaming)
for k in range(len(jointHandle)):
_, jointConfig[k] = vrep.simxGetJointPosition(clientID, jointHandle[k], vrep.simx_opmode_streaming)
#print(jointConfig[k])
_, linear_velocity, angular_velocity = vrep.simxGetObjectVelocity(clientID, baseHandle, vrep.simx_opmode_streaming)
for k in range(len(jointHandle)):
_, jointVelocity[k] = vrep.simxGetObjectFloatParameter(clientID, jointHandle[k], 2012, vrep.simx_opmode_streaming)
#print(jointConfig)
#print(base_pos)
x_init[0] = base_pos[2]
x_init[1] = linear_velocity[2]
p_error = j_target-jointConfig[2]
d_error = 0-jointVelocity[2]
# 摆动期 kp=10, kd=1
# 支撑期 kp=10, kd=1
control_force[1] = (40*p_error + 3*d_error)
control_force[0] = 0.01
print(control_force[1])