def wait_till_stream(self):
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, ori = vrep.simxGetObjectOrientation(self.client_id, self.body_handle, -1, vrep.simx_opmode_buffer)
self.logger.info("orient:%s" % str(ori))
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1, vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetJointPosition(self.client_id, self.left_joint, vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetJointPosition(self.client_id, self.right_joint, vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.right_joint, 2012, vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.right_joint, 2012, vrep.simx_opmode_buffer)
def get_vision_sensor_info_from_name(self, vision_sensor_name):
#returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, vision_sensor_name, self.operation_mode)
returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, vision_sensor_name, vrep.simx_opmode_oneshot_wait)
# Skip non-existent objects
if (returnCode != 0):
return None
returnCode, param_near_clipping = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1000, self.operation_mode)
returnCode, param_far_clipping = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1001, self.operation_mode)
returnCode, param_resolution_x = vrep.simxGetObjectIntParameter(self.clientID, obj_handle, 1002, self.operation_mode)
returnCode, param_resolution_y = vrep.simxGetObjectIntParameter(self.clientID, obj_handle, 1003, self.operation_mode)
returnCode, param_perspective_angle = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1004, self.operation_mode)
try:
ratio = param_resolution_x/param_resolution_y
except ZeroDivisionError:
return None
if (ratio > 1):
param_perspective_angle_x = param_perspective_angle
param_perspective_angle_y = 2*np.arctan(np.tan(param_perspective_angle/2)/ratio)
else:
param_perspective_angle_x = 2*np.arctan(np.tan(param_perspective_angle/2)*ratio)
param_perspective_angle_y = param_perspective_angle
return VisionSensor(vision_sensor_name, \
param_resolution_x, \
param_resolution_y, \
param_perspective_angle, \
param_perspective_angle_x, \
param_perspective_angle_y, \
param_near_clipping, \
param_far_clipping, \
obj_handle)
def render(self, position):
margin = -0.35
# print ("robot pose: ", position)
if self.robot_sim is None:
# x, self.robot_sim = vrep.simxGetObjectHandle(self.clientID, "Quadricopter", vrep.simx_opmode_blocking)
# x, self.robot_sim = vrep.simxLoadModel(self.clientID, self.models_path + "Quadricopter.ttm", 0, vrep.simx_opmode_blocking)
x, self.robot_sim = vrep.simxGetObjectHandle(
self.clientID, "youBot", vrep.simx_opmode_blocking)
# x, self.target = vrep.simxLoadModel(self.clientID, self.models_path + "KUKA YouBot.ttm", 0, vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.clientID, self.robot_sim, -1, (position[0], position[1], position[2] + margin + \
math.fabs(vrep.simxGetObjectFloatParameter (self.clientID, self.robot_sim, vrep.sim_objfloatparam_modelbbox_min_z, vrep.simx_opmode_blocking)[1])), vrep.simx_opmode_oneshot)
#x, self.target = vrep.simxGetObjectHandle(self.clientID, "Quadricopter_target", vrep.simx_opmode_blocking) ##quad
x, self.target = vrep.simxGetObjectHandle(
self.clientID, "target", vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.clientID, self.target, -1, (position[0], position[1], position[2] + margin + \
math.fabs(vrep.simxGetObjectFloatParameter (self.clientID, self.robot_sim, vrep.sim_objfloatparam_modelbbox_min_z, vrep.simx_opmode_blocking)[1])), vrep.simx_opmode_oneshot)
x, self.target_hall = vrep.simxGetObjectHandle(
self.clientID, "inflated_convex_hall_target",
vrep.simx_opmode_blocking)
# vrep.simxSetObjectPosition(self.clientID, self.target, -1, (position[0], position[1] + 1, position[2] + margin + \
# math.fabs(vrep.simxGetObjectFloatParameter (self.clientID, self.robot_sim, vrep.sim_objfloatparam_modelbbox_min_z, vrep.simx_opmode_blocking)[1])), vrep.simx_opmode_oneshot)
# vrep.simxSetObjectParent(self.clientID,self.target,-1,True, vrep.simx_opmode_blocking)
# x, self.goal_robot = vrep.simxLoadModel(self.clientID, self.models_path + "KUKA YouBot.ttm", 0, vrep.simx_opmode_blocking)
# x, self.goal_robot = vrep.simxGetObjectHandle(self.clientID, "Quadricopter#0", vrep.simx_opmode_blocking)
# simxInt simxSetModelProperty(self.clientID,self.goal_robot,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)
##sim.modelproperty_not_visible
else:
pass
def get_dynamic_obj_info(self):
dynamic_obj_list = []
for obj_name in self.dynamic_obj_name_list:
#returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, obj_name, self.operation_mode)
returnCode, obj_handle = vrep.simxGetObjectHandle(
self.clientID, obj_name, vrep.simx_opmode_oneshot_wait)
# Skip non-existent objects
if (returnCode != 0):
continue
# Get pose and orientation
returnCode, obj_pose = vrep.simxGetObjectPosition(
self.clientID, obj_handle, -1, self.operation_mode)
returnCode, obj_ori = vrep.simxGetObjectOrientation(
self.clientID, obj_handle, -1, self.operation_mode)
# Get the velocity of the obstacle
returnCode, param_vel_x = vrep.simxGetObjectFloatParameter(
self.clientID, obj_handle, 11, self.operation_mode)
returnCode, param_vel_y = vrep.simxGetObjectFloatParameter(
self.clientID, obj_handle, 12, self.operation_mode)
returnCode, param_vel_z = vrep.simxGetObjectFloatParameter(
self.clientID, obj_handle, 13, self.operation_mode)
returnCode, param_vel_r = vrep.simxGetObjectFloatParameter(
self.clientID, obj_handle, 14, self.operation_mode)
obj_vel = np.array(
[param_vel_x, param_vel_y, param_vel_z, param_vel_r])
self.dynamic_obj_list = dynamic_obj_list
def retrieve_state(self):
pos = np.zeros(2)
vel = np.zeros(2)
tip = np.zeros(3)
_, pos[0] = vrep.simxGetJointPosition(self.clientID,
self.jointHandles[0],
vrep.simx_opmode_blocking)
_, pos[1] = vrep.simxGetJointPosition(self.clientID,
self.jointHandles[1],
vrep.simx_opmode_blocking)
_, vel[0] = vrep.simxGetObjectFloatParameter(self.clientID,
self.jointHandles[0],
2012,
vrep.simx_opmode_blocking)
_, vel[1] = vrep.simxGetObjectFloatParameter(self.clientID,
self.jointHandles[1],
2012,
vrep.simx_opmode_blocking)
_, tip_list = vrep.simxGetObjectPosition(self.clientID, self.tipHandle,
-1, vrep.simx_opmode_blocking)
tip = np.asarray(tip_list)
return {1: pos, 2: vel, 3: tip}
def get_vision_sensor_info_from_name(self, vision_sensor_name):
returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, vision_sensor_name, self.operation_mode)
if (returnCode == 0):
returnCode, param_near_clipping = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1000, self.operation_mode)
returnCode, param_far_clipping = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1001, self.operation_mode)
returnCode, param_resolution_x = vrep.simxGetObjectIntParameter(self.clientID, obj_handle, 1002, self.operation_mode)
returnCode, param_resolution_y = vrep.simxGetObjectIntParameter(self.clientID, obj_handle, 1003, self.operation_mode)
returnCode, param_perspective_angle = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 1004, self.operation_mode)
ratio = param_resolution_x/param_resolution_y
if (ratio > 1):
param_perspective_angle_x = param_perspective_angle
param_perspective_angle_y = 2*np.arctan(np.tan(param_perspective_angle/2)/ratio)
else:
param_perspective_angle_x = 2*np.arctan(np.tan(param_perspective_angle/2)*ratio)
param_perspective_angle_y = param_perspective_angle
return VisionSensor(vision_sensor_name, \
param_resolution_x, \
param_resolution_y, \
param_perspective_angle, \
param_perspective_angle_x, \
param_perspective_angle_y, \
param_near_clipping, \
param_far_clipping, \
obj_handle)
def update_dynamic_obj_info(self):
for dynamic_obj in (self.dynamic_obj_list + self.robot_obj_list):
obj_handle = dynamic_obj.handle
# Get pose and orientation
returnCode, obj_pose = vrep.simxGetObjectPosition(self.clientID, obj_handle, -1, self.operation_mode)
returnCode, obj_ori = vrep.simxGetObjectOrientation(self.clientID, obj_handle, -1, self.operation_mode)
# Get the velocity of the obstacle
returnCode, param_vel_x = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 11, self.operation_mode)
returnCode, param_vel_y = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 12, self.operation_mode)
returnCode, param_vel_z = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 13, self.operation_mode)
returnCode, param_vel_r = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 14, self.operation_mode)
obj_vel = np.array([param_vel_x, param_vel_y, param_vel_z, param_vel_r])
obj_size = dynamic_obj.size
bbox_min = [obj_pose[0]-obj_size[0]/2.0, obj_pose[1]-obj_size[1]/2.0, obj_pose[2]-obj_size[2]/2.0]
bbox_max = [obj_pose[0]+obj_size[0]/2.0, obj_pose[1]+obj_size[1]/2.0, obj_pose[2]+obj_size[2]/2.0]
# TODO: must check if obj is really updating
dynamic_obj.pose = obj_pose
dynamic_obj.ori = obj_ori
dynamic_obj.vel = obj_vel
dynamic_obj.bbox_min = bbox_min
dynamic_obj.bbox_max = bbox_max
def handle_output(self):
# return whatever state information you need to get the error you want
# this will get you the information for the center of the object. If you
# want something like the position of the end of an arm, you will need
# to do some calculations, or just make a dummy object, attach it to
# the point you want, and get the position of the dummy object
#err, hand_pos = vrep.simxGetObjectPosition(self.cid, self.hand, -1,
# vrep.simx_opmode_oneshot)
#err, hand_ori = vrep.simxGetObjectOrientation(self.cid, self.hand, -1,
# vrep.simx_opmode_oneshot)
err, hand_ori = vrep.simxGetJointPosition(self.cid, self.hand_joint,
vrep.simx_opmode_oneshot)
err, arm_ori = vrep.simxGetJointPosition(self.cid, self.arm_joint,
vrep.simx_opmode_oneshot)
err, hand_vel = vrep.simxGetObjectFloatParameter(self.cid, self.hand_joint,
2012, vrep.simx_opmode_oneshot)
err, arm_vel = vrep.simxGetObjectFloatParameter(self.cid, self.arm_joint,
2012, vrep.simx_opmode_oneshot)
err, hand_pos = vrep.simxGetObjectPosition(self.cid, self.hand, -1,
vrep.simx_opmode_oneshot)
err, target_pos = vrep.simxGetObjectPosition(self.cid, self.target, -1,
vrep.simx_opmode_oneshot)
return [arm_ori, hand_ori, arm_vel, hand_vel, hand_pos[0], hand_pos[2],
target_pos[0], target_pos[1]]
def getWheelVelocity(self):
err, left_w = vrep.simxGetObjectFloatParameter(
self.clientID, self.motor_left,
vrepConst.sim_jointfloatparam_velocity, vrep.simx_opmode_blocking)
err, right_w = vrep.simxGetObjectFloatParameter(
self.clientID, self.motor_right,
vrepConst.sim_jointfloatparam_velocity, vrep.simx_opmode_blocking)
return (left_w, right_w)
def get_info(self):
# Check velocity
err, bl_wheel_vel = vrep.simxGetObjectFloatParameter(self.clientID, self.bl_brake_handle, vrep.sim_jointfloatparam_velocity, vrep.simx_opmode_streaming);
err, br_wheel_vel = vrep.simxGetObjectFloatParameter(self.clientID, self.br_brake_handle, vrep.sim_jointfloatparam_velocity, vrep.simx_opmode_streaming);
rear_wheel_velocity = ((bl_wheel_vel) + (br_wheel_vel))/2.0;
linear_velocity = rear_wheel_velocity * 0.09 * 3.6; # Kmph
throttle = linear_velocity;
steer_errorCode, steer_pos = vrep.simxGetJointPosition(self.clientID, self.steer_handle, vrep.simx_opmode_streaming);
if(self.printFlag):
print('Throttle:', throttle, 'Steering:', steer_pos);
def get_all_objects_info(self):
obj_name_list = self.static_obj_name_list + self.dynamic_obj_name_list + self.robot_name_list
obj_list = []
for obj_name in obj_name_list:
returnCode, obj_handle = vrep.simxGetObjectHandle(self.clientID, obj_name, self.operation_mode)
# Get pose and orientation
returnCode, obj_pose = vrep.simxGetObjectPosition(self.clientID, obj_handle, -1, self.operation_mode)
returnCode, obj_ori = vrep.simxGetObjectOrientation(self.clientID, obj_handle, -1, self.operation_mode)
# Get size
returnCode, param_min_x = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 21, self.operation_mode)
returnCode, param_min_y = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 22, self.operation_mode)
returnCode, param_min_z = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 23, self.operation_mode)
returnCode, param_max_x = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 24, self.operation_mode)
returnCode, param_max_y = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 25, self.operation_mode)
returnCode, param_max_z = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 26, self.operation_mode)
size_x = param_max_x - param_min_x
size_y = param_max_y - param_min_y
size_z = param_max_z - param_min_z
obj_size = np.array([size_x, size_y, size_z])
bbox_min = [obj_pose[0]-size_x/2.0, obj_pose[1]-size_y/2.0, obj_pose[2]-size_z/2.0]
bbox_max = [obj_pose[0]+size_x/2.0, obj_pose[1]+size_y/2.0, obj_pose[2]+size_z/2.0]
# Get velocity
returnCode, param_vel_x = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 11, self.operation_mode)
returnCode, param_vel_y = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 12, self.operation_mode)
returnCode, param_vel_z = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 13, self.operation_mode)
returnCode, param_vel_r = vrep.simxGetObjectFloatParameter(self.clientID, obj_handle, 14, self.operation_mode)
obj_vel = np.array([param_vel_x, param_vel_y, param_vel_z, param_vel_r])
# Add object to static, dynamic or robot list
if (obj_name in self.static_obj_name_list):
obj = SceneObject(obj_name, obj_pose, obj_ori, obj_size, obj_vel, bbox_min, bbox_max, obj_handle)
self.static_obj_list.append(obj)
elif (obj_name in self.dynamic_obj_name_list):
obj = SceneObject(obj_name, obj_pose, obj_ori, obj_size, obj_vel, bbox_min, bbox_max, obj_handle)
self.dynamic_obj_list.append(obj)
else:
obj = Robot(obj_name, obj_pose, obj_ori, obj_size, obj_vel, bbox_min, bbox_max, obj_handle)
self.get_robot_vision_sensor_info(obj)
self.robot_obj_list.append(obj)
obj_list = self.static_obj_list + self.dynamic_obj_list + self.robot_obj_list
return obj_list
def signal_values(self):
vrep.simxGetObjectOrientation(self.client_id, self.body_handle, -1,
vrep.simx_opmode_streaming)
vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1,
vrep.simx_opmode_streaming)
for handle_idx in range(0, self.joint_count):
vrep.simxGetObjectFloatParameter(self.client_id,
self.handles[handle_idx], 2012,
vrep.simx_opmode_streaming)
for handle_idx in range(0, self.joint_count):
vrep.simxGetJointPosition(self.client_id, self.handles[handle_idx],
vrep.simx_opmode_streaming)
def render(self, table, sim_env):
position = [
table.get_feature("x").value - env_margin,
table.get_feature("y").value - env_margin, 0
]
x, sim_table = vrep.simxLoadModel(self.clientID,
self.models_path + "diningTable.ttm",
0, vrep.simx_opmode_blocking)
# x = vrep.simxSetObjectParent(self.clientID, sim_table, sim_env, True, vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.clientID, sim_table, -1, (position[0], position[1], position[2] + \
vrep.simxGetObjectFloatParameter (self.clientID, sim_table, vrep.sim_objfloatparam_modelbbox_max_x, vrep.simx_opmode_blocking)[1]), vrep.simx_opmode_oneshot)
# vrep.simxSetModelProperty(self.clientID,sim_table,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)
#returnCode, prop = vrep.simxGetModelProperty(self.clientID,sim_table,vrep.simx_opmode_blocking)
#returnCode2 = vrep.simxSetModelProperty(self.clientID, sim_table, prop + vrep.sim_modelproperty_not_respondable, vrep.simx_opmode_blocking)
#returnCode3, prop = vrep.simxGetModelProperty(self.clientID, sim_table, vrep.simx_opmode_blocking)
#print (prop)
offset_z = math.fabs(
vrep.simxGetObjectFloatParameter(
self.clientID, sim_table,
vrep.sim_objfloatparam_modelbbox_min_z,
vrep.simx_opmode_blocking)[1])
offset_y = math.fabs(
vrep.simxGetObjectFloatParameter(
self.clientID, sim_table,
vrep.sim_objfloatparam_modelbbox_min_y,
vrep.simx_opmode_blocking)[1])
margin = 0.2
bill_margin = 0.05
self.chairs_offset = [(0, offset_y + margin, 0, 0, 0, 0),
(0, -offset_y - margin, 0, 0, 0, math.pi),
(offset_z + margin, 0, 0, 0, 0, -math.pi / 2),
(-offset_z - margin, 0, 0, 0, 0, math.pi / 2)]
self.humans_offset = [
(0, offset_y - margin, bill_margin, 0, 0, -math.pi / 2),
(0, -offset_y + margin, bill_margin, 0, 0, math.pi / 2),
(offset_z - margin, 0, bill_margin, 0, 0, math.pi),
(-offset_z + margin, 0, bill_margin, 0, 0, 0)
]
for i in range(len(table.humans)):
sim_chair = self.render_chair(sim_table, position,
self.chairs_offset[i])
self.chairs.append(sim_chair)
self.humans.append(
self.render_human(sim_chair, position, self.humans_offset[i]))
self.sim_table = sim_table
return sim_table
def getVirtualCamAdditionalMatrix(sensorHandle):
"获得深度相机的近景/远景距离/视场角/分辨率"
err, nearClip = vrep.simxGetObjectFloatParameter(
clientID, sensorHandle, vrep.sim_visionfloatparam_near_clipping, vrep.simx_opmode_oneshot_wait)
err, farClip = vrep.simxGetObjectFloatParameter(
clientID, sensorHandle, vrep.sim_visionfloatparam_far_clipping, vrep.simx_opmode_oneshot_wait)
err, angle = vrep.simxGetObjectFloatParameter(
clientID, sensorHandle, vrep.sim_visionfloatparam_perspective_angle, vrep.simx_opmode_oneshot_wait)
err, res_x = vrep.simxGetObjectIntParameter(
clientID, sensorHandle, vrep.sim_visionintparam_resolution_x, vrep.simx_opmode_oneshot_wait)
err, res_y = vrep.simxGetObjectIntParameter(
clientID, sensorHandle, vrep.sim_visionintparam_resolution_y, vrep.simx_opmode_oneshot_wait)
return nearClip, farClip, angle, (res_x, res_y)
def get_maze_segments(wallHandles):
## Get handles to:
# 1. The Maze collection:
res, mazeHandle = vrep.simxGetCollectionHandle(clientID, "Maze",
vrep.simx_opmode_blocking)
# Get the handles associated with each wall and the absolute position of the center of each wall:
res, wallHandles, intData, absPositions, stringData = vrep.simxGetObjectGroupData(
clientID, mazeHandle, 3, vrep.simx_opmode_blocking)
mazeSegments = []
if res == vrep.simx_return_ok:
count = 1
for wall in wallHandles:
res, wallCenterAbsPos = vrep.simxGetObjectPosition(
clientID, wall, -1, vrep.simx_opmode_oneshot_wait)
res, wallMinY = vrep.simxGetObjectFloatParameter(
clientID, wall, vrep.sim_objfloatparam_objbbox_min_y,
vrep.simx_opmode_oneshot_wait)
res, wallMaxY = vrep.simxGetObjectFloatParameter(
clientID, wall, vrep.sim_objfloatparam_objbbox_max_y,
vrep.simx_opmode_oneshot_wait)
wallLength = abs(wallMaxY - wallMinY)
# Get the orientation of the wall: Third euler angle is the angle around z-axis:
res, wallOrient = vrep.simxGetObjectOrientation(
clientID, wall, -1, vrep.simx_opmode_oneshot_wait)
# Get the end points of the maze wall: A list containing two tuples: [ (xs,ys) , (xe,ye)]
wallSeg = get_wall_seg(
wallCenterAbsPos, wallLength, wallOrient[2]
) # Assuming all walls are on the ground and nearly flat:
print("Wall #", count, " -> ", wallSeg)
mazeSegments.append(wallSeg)
count += 1
else:
print(" Failed to get individual wall handles!")
return mazeSegments
def render_human(self, chair, position, offset):
x, person = vrep.simxLoadModel(self.clientID,
self.models_path + "Sitting Bill.ttm",
0, vrep.simx_opmode_blocking)
x = vrep.simxSetObjectParent(self.clientID, person, chair, True,
vrep.simx_opmode_blocking)
offset_z = vrep.simxGetObjectFloatParameter(
self.clientID, person, vrep.sim_objfloatparam_modelbbox_max_z,
vrep.simx_opmode_blocking)[1]
vrep.simxSetObjectPosition(self.clientID, person, -1,
(position[0] + offset[0], position[1] +
offset[1], position[2] + offset[2]),
vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.clientID, person, -1,
(offset[3], offset[4], offset[5]),
vrep.simx_opmode_oneshot)
# vrep.simxSetModelProperty(self.clientID,person,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)
# x, objs = vrep.simxGetObjects(self.clientID, vrep.sim_handle_all, vrep.simx_opmode_oneshot_wait)
# x, joint = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftElbowJoint', vrep.simx_opmode_oneshot_wait)
# x, shoulder = vrep.simxGetObjectHandle(self.clientID, 'Bill_leftShoulderJoint', vrep.simx_opmode_oneshot_wait)
# rotationMatrix = (-1,0,0,0,0,-1,0,0,0,0,-1,0)
# x = vrep.simxSetSphericalJointMatrix(self.clientID, shoulder, rotationMatrix,vrep.simx_opmode_streaming)
#Moving Rotational Joint
# ang = -math.pi/2
# x = vrep.simxSetJointPosition(self.clientID, joint, ang, vrep.simx_opmode_oneshot)
# x, pos = vrep.simxGetJointPosition(self.clientID, joint, vrep.simx_opmode_streaming)
return person
def _get_observation(self): # get current observations s_t
state = np.zeros(self.obs_dims)
# state is [θ1, θ2, θ3, θ4, θ5, θ6, ω1, ω2, ω3, ω4, ω5, ω6,
# Base_x, Base_y, Base_z, Base_α, Base_β, Base_γ,
# Vx, Vy, Vz, dα, dβ, dγ]
Joint_angle = [vrep.simxGetJointPosition(self.clientID, self.handles['joint'][i],
vrep.simx_opmode_blocking)[1]
for i in range(6)] # retrieve the rotation angle as [θ1, θ2, θ3, θ4, θ5, θ6]
state[0: 6] = Joint_angle
Joint_velocity = [vrep.simxGetObjectFloatParameter(self.clientID, self.handles['joint'][i],
2012, vrep.simx_opmode_blocking)[1]
for i in range(6)] # retrieve the joint velocity as [ω1, ω2, ω3, ω4, ω5, ω6]
state[6: 12] = Joint_velocity
Base_position = [vrep.simxGetObjectPosition(self.clientID, self.handles['base'][0],
-1, vrep.simx_opmode_blocking)[1]]
state[12: 15] = Base_position[0] # retrieve the position of Base as [Base_x, Base_y, Base_z]
Base_pose = [vrep.simxGetObjectOrientation(self.clientID, self.handles['base'][0],
-1, vrep.simx_opmode_blocking)[1]]
state[15: 18] = Base_pose[0] # retrieve the orientation of Base as [Base_α, Base_β, Base_γ]
_, Base_Vel, Base_Ang_Vel = vrep.simxGetObjectVelocity(self.clientID, self.handles['base'][0],
vrep.simx_opmode_blocking)
state[18: 21] = Base_Vel # retrieve the linear velocity of Base as [Vx, Vy, Vz]
state[21: 24] = Base_Ang_Vel # retrieve the angular velocity of Base as [dα, dβ, dγ]
state = np.asarray(state)
return state
def get_joint_vel(self):
jvel = np.zeros((1, self.joint_count))
for handle_idx in range(0, self.joint_count):
_, jvel[0, handle_idx] = vrep.simxGetObjectFloatParameter(
self.client_id, self.handles[handle_idx], 2012,
vrep.simx_opmode_buffer)
return jvel
def get_feedback(self):
""" Return a dictionary of information needed by the controller.
Returns the joint angles and joint velocities in [rad] and [rad/sec],
respectively
"""
for ii, joint_handle in enumerate(self.joint_handles):
# get the joint angles
_, self.q[ii] = vrep.simxGetJointPosition(
self.clientID,
joint_handle,
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception('Error retrieving joint angle, ' +
'return code ', _)
# get the joint velocity
_, self.dq[ii] = vrep.simxGetObjectFloatParameter(
self.clientID,
joint_handle,
2012, # ID for joint angular velocity
vrep.simx_opmode_blocking)
if _ != 0:
raise Exception('Error retrieving joint velocity, ' +
'return code ', _)
return {'q': self.q,
'dq': self.dq}
def get_near_clip_plane(self, prec=None):
"""Retrieve near clipping plane."""
if self._handle < 0:
if self._handle == MISSING_HANDLE:
raise RuntimeError(
"Could not retrieve near clipping plane of {}: missing "
"name or handle.".format(self._name))
if self._handle == REMOVED_OBJ_HANDLE:
raise RuntimeError(
"Could not retrieve near clipping plane of {}: object "
"removed.".format(self._name))
client_id = self.client_id
if client_id is None:
raise ConnectionError(
"Could not retrieve near clipping plane of {}: not connected "
"to V-REP remote API server.".format(self._name))
res, clip_plane = vrep.simxGetObjectFloatParameter(
client_id, self._handle, vrep.sim_visionfloatparam_near_clipping,
vrep.simx_opmode_blocking)
if res != vrep.simx_return_ok:
raise ServerError("Could not retrieve near clipping plane of {}."
"".format(self._name))
if prec is not None:
clip_plane = round(clip_plane, prec) # near clipping plane may be
# slightly imprecise due to
# the use of single-precision
# floating-point format by
# V-REP
return clip_plane
def retrieve_state(self):
pos = np.zeros(1)
vel = np.zeros(1)
tip = np.zeros(3)
_, pos[0] = vrep.simxGetJointPosition(self.clientID,
self.jointHandles[0],
vrep.simx_opmode_blocking)
_, vel[0] = vrep.simxGetObjectFloatParameter(self.clientID,
self.jointHandles[0],
2012,
vrep.simx_opmode_blocking)
_, tip_list = vrep.simxGetObjectPosition(self.clientID, self.tipHandle,
-1, vrep.simx_opmode_blocking)
tip = np.asarray(tip_list)
_, _1, jac_list, _2, _3 = vrep.simxCallScriptFunction(
self.clientID, 'Jacobian_Script',
vrep.sim_scripttype_childscript, 'jacobian_retriever', [], [], [],
bytearray(), vrep.simx_opmode_blocking)
jac = np.asarray(jac_list)
jac = np.reshape(jac, (6, 1))
jac = jac[0:3, :]
return {1: pos, 2: vel, 3: tip, 5: jac}
def get_wheel_vel(self):
"""Returns the wheel velocities as a list of (left,right)."""
vels = []
for loc in self.wj_locs:
_,vel = vrep.simxGetObjectFloatParameter(self.cid, self.wjs[loc], vrep.sim_jointfloatparam_velocity, self.op)
vels.append(vel)
return vels
def wait_till_stream(self):
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectOrientation(self.client_id,
self.body_handle, -1,
vrep.simx_opmode_buffer)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectPosition(self.client_id,
self.body_handle, -1,
vrep.simx_opmode_buffer)
for handle_idx in range(0, self.joint_count):
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectFloatParameter(
self.client_id, self.handles[handle_idx], 2012,
vrep.simx_opmode_buffer)
for handle_idx in range(0, self.joint_count):
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetJointPosition(self.client_id,
self.handles[handle_idx],
vrep.simx_opmode_buffer)
def get_object_height(self, handle):
time.sleep(self.sleep_sec_min)
err, minval = vrep.simxGetObjectFloatParameter(self.clientID,
handle, vrep.sim_objfloatparam_modelbbox_min_z,
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError("get_object_height(): Failed to get parameters: sim_objfloatparam_modelbbox_min_z")
err, maxval = vrep.simxGetObjectFloatParameter(self.clientID,
handle, vrep.sim_objfloatparam_modelbbox_max_z,
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError("get_object_height(): Failed to get parameters: sim_objfloatparam_modelbbox_max_z")
return maxval - minval
def render_chair(self, table, position, offset):
x, chair = vrep.simxLoadModel(self.clientID,
self.models_path + "dining chair.ttm", 0,
vrep.simx_opmode_blocking)
x = vrep.simxSetObjectParent(self.clientID, chair, table, True,
vrep.simx_opmode_blocking)
offset_z = math.fabs(
vrep.simxGetObjectFloatParameter(
self.clientID, chair, vrep.sim_objfloatparam_modelbbox_min_z,
vrep.simx_opmode_blocking)[1])
vrep.simxSetObjectPosition(
self.clientID, chair, -1,
(position[0] + offset[0], position[1] + offset[1],
position[2] + offset_z + offset[2]), vrep.simx_opmode_oneshot)
vrep.simxSetObjectOrientation(self.clientID, chair, -1,
(offset[3], offset[4], offset[5]),
vrep.simx_opmode_oneshot)
#returnCode, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking)
# print (prop, prop + vrep.sim_modelproperty_not_respondable + vrep.sim_modelproperty_not_collidable)
#returnCode2 = vrep.simxSetModelProperty(self.clientID, chair, prop + vrep.sim_modelproperty_not_respondable, vrep.simx_opmode_blocking)
#returnCode3, prop = vrep.simxGetModelProperty(self.clientID,chair,vrep.simx_opmode_blocking)
# print (returnCode, returnCode2, returnCode3, prop)
# vrep.simxSetModelProperty(self.clientID,chair,vrep.sim_modelproperty_not_collidable,vrep.simx_opmode_oneshot)
return chair
def __init__(self, robot):
self.id = vrep.simxStart('127.0.0.1', 19997, True, True, 1000, 5)
if self.id == -1:
exit('Connection to V-REP failed.')
self.running = False
self.joints = []
for joint in robot['joints']:
self.joints.append(
vrep.simxGetObjectHandle(self.id, joint,
vrep.simx_opmode_blocking)[1])
vrep.simxGetJointPosition(self.id, self.joints[-1],
vrep.simx_opmode_streaming)
vrep.simxGetObjectFloatParameter(self.id, self.joints[-1],
vrep.sim_jointfloatparam_velocity,
vrep.simx_opmode_streaming)
self.distances = {}
self.dummies = {}
def __init__(self, client_id: int, handle: int, name: str):
"""Initialize sensor and get specific information
Initialize
"""
super().__init__(client_id, handle, name)
# Assume that the sensor returns a square image
__, self.res = vrep.simxGetObjectIntParameter(
client_id, handle, vrep.sim_visionintparam_resolution_x,
self.BLOCK)
__, self.max_depth = vrep.simxGetObjectFloatParameter(
client_id, handle, vrep.sim_visionfloatparam_far_clipping,
self.BLOCK)
__, angle_radians = vrep.simxGetObjectFloatParameter(
client_id, handle, vrep.sim_visionfloatparam_perspective_angle,
self.BLOCK)
self.angle = round(degrees(angle_radians), 3)
def get_bbox(self) -> Tuple[np.ndarray, np.ndarray]:
coords = tuple(
vrep.simxGetObjectFloatParameter(self.client_id, self.handle, i,
self.BLOCK)[1]
for i in range(15, 21))
return np.array(coords[:3], np.float32), \
np.array(coords[3:], np.float32)
def object_properties(cls, com, handle):
res, min_x = remote_api.simxGetObjectFloatParameter(
com.api_id, handle, 21, remote_api.simx_opmode_oneshot_wait)
assert res == 0
min_x = 100 * min_x
res, max_x = remote_api.simxGetObjectFloatParameter(
com.api_id, handle, 24, remote_api.simx_opmode_oneshot_wait)
assert res == 0
res, min_y = remote_api.simxGetObjectFloatParameter(
com.api_id, handle, 22, remote_api.simx_opmode_oneshot_wait)
min_y = 100 * min_y
assert res == 0
res, max_y = remote_api.simxGetObjectFloatParameter(
com.api_id, handle, 25, remote_api.simx_opmode_oneshot_wait)
max_x = 100 * max_x
assert res == 0
res, min_z = remote_api.simxGetObjectFloatParameter(
com.api_id, handle, 23, remote_api.simx_opmode_oneshot_wait)
min_z = 100 * min_z
assert res == 0
res, max_z = remote_api.simxGetObjectFloatParameter(
com.api_id, handle, 26, remote_api.simx_opmode_oneshot_wait)
max_z = 100 * max_z
assert res == 0
res, mass = remote_api.simxGetObjectFloatParameter(
com.api_id, handle, 3005, remote_api.simx_opmode_oneshot_wait)
assert res == 0
dims = (max_x - min_x, max_y - min_y, max_z - min_z)
return dims, mass
def object_properties(cls, com, handle):
res, min_x = remote_api.simxGetObjectFloatParameter(com.api_id, handle, 21,
remote_api.simx_opmode_oneshot_wait)
assert res == 0
min_x = 100*min_x
res, max_x = remote_api.simxGetObjectFloatParameter(com.api_id, handle, 24,
remote_api.simx_opmode_oneshot_wait)
assert res == 0
res, min_y = remote_api.simxGetObjectFloatParameter(com.api_id, handle, 22,
remote_api.simx_opmode_oneshot_wait)
min_y = 100*min_y
assert res == 0
res, max_y = remote_api.simxGetObjectFloatParameter(com.api_id, handle, 25,
remote_api.simx_opmode_oneshot_wait)
max_x = 100*max_x
assert res == 0
res, min_z = remote_api.simxGetObjectFloatParameter(com.api_id, handle, 23,
remote_api.simx_opmode_oneshot_wait)
min_z = 100*min_z
assert res == 0
res, max_z = remote_api.simxGetObjectFloatParameter(com.api_id, handle, 26,
remote_api.simx_opmode_oneshot_wait)
max_z = 100*max_z
assert res == 0
res, mass = remote_api.simxGetObjectFloatParameter(com.api_id, handle, 3005,
remote_api.simx_opmode_oneshot_wait)
assert res == 0
dims = (max_x - min_x, max_y - min_y, max_z - min_z)
return dims, mass
def handle_output(self):
err, arm_ori = vrep.simxGetJointPosition(self.cid, self.arm_joint,
vrep.simx_opmode_oneshot)
#2012 is the code for joint velocity
err, arm_vel = vrep.simxGetObjectFloatParameter(self.cid,
self.arm_joint, 2012,
vrep.simx_opmode_oneshot)
return [arm_ori, arm_vel]
def get_object_height(self, handle):
time.sleep(self.sleep_sec_min)
err, minval = vrep.simxGetObjectFloatParameter(
self.clientID, handle, vrep.sim_objfloatparam_modelbbox_min_z,
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError(
"get_object_height(): Failed to get parameters: sim_objfloatparam_modelbbox_min_z"
)
err, maxval = vrep.simxGetObjectFloatParameter(
self.clientID, handle, vrep.sim_objfloatparam_modelbbox_max_z,
vrep.simx_opmode_blocking)
if err != vrep.simx_return_ok:
raise RuntimeError(
"get_object_height(): Failed to get parameters: sim_objfloatparam_modelbbox_max_z"
)
return maxval - minval
def signal_values(self):
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectOrientation(self.client_id, self.body_handle, -1, vrep.simx_opmode_streaming)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectPosition(self.client_id, self.body_handle, -1, vrep.simx_opmode_streaming)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetJointPosition(self.client_id, self.left_joint, vrep.simx_opmode_streaming)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetJointPosition(self.client_id, self.right_joint, vrep.simx_opmode_streaming)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.left_joint, 2012, vrep.simx_opmode_streaming)
ret = vrep.simx_return_illegal_opmode_flag
while ret != vrep.simx_return_ok:
ret, _ = vrep.simxGetObjectFloatParameter(self.client_id, self.right_joint, 2012,
vrep.simx_opmode_streaming)
def getRobotState(self):
pos = np.zeros(len(self.joints))
vel = np.zeros(len(self.joints))
vrep.simxPauseCommunication(self.id, True)
for i, joint in enumerate(self.joints):
pos[i] = vrep.simxGetJointPosition(self.id, joint,
vrep.simx_opmode_buffer)[1]
vel[i] = vrep.simxGetObjectFloatParameter(
self.id, joint, vrep.sim_jointfloatparam_velocity,
vrep.simx_opmode_buffer)[1]
vrep.simxPauseCommunication(self.id, False)
return pos, vel
def callback(data):
print('inside callback')
data = data.data
#print(data)
#a,b,c = calc_parabola_vertex(data[0], data[1], data[2], data[3], data[4], data[5])
a,b = calc_straight_line(data[0], data[1], data[4], data[5])
print([a,b])
z_req_vision= ((a*(x_req))+(b))/2
# z_req_vision= (a*(x_req**2))+(b*x_req)+c
print('z required from vision'+str(z_req_vision))
print('x required from vision'+str(x_req))
print('-'*8+'Checking if ball can be blocked'+'-'*8)
transformation = np.array([[1,0,0,x_req*1000],[0,1,0,0],[0,0,1,z_req_vision*1000],[0,0,0,1]])
desired_angles = robot.inverseKinematics(transformation)
if desired_angles:
print('-'*8+'Moving'+'-'*8)
error = desired_angles-angles
#error = np.array(error)
error_collect.append(error)
#error_collect.append(error.reshape(3,1))
sum = np.sum(np.abs(error))
Kp = [1000*(sum-np.abs(i))/sum for i in error]
Kv = 100
velocity_error = np.array([0 ,0 ,0])
desired_velocity = np.copy(velocity_error)
for i in range(3):
while abs(error[i])>epsilon:
vrep.simxSetJointTargetVelocity(clientID,joints[i],error[i]*9999/abs(error[i]),vrep.simx_opmode_oneshot)
# if error[i]>0.5:
# vrep.simxSetJointForce(clientID,joints[i],abs(Kp*0.5)+(Kv*velocity_error[i]),vrep.simx_opmode_oneshot)
# else:#+(Kv*velocity_error[i])
if abs(error[i])<epsilon:
vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],abs(Kp[i]*(error[i]))+Kv*velocity_error[i],vrep.simx_opmode_oneshot)
error[i] = desired_angles[i] - (vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
#print(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
if abs(error[i])<epsilon:
vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
res,velocity = vrep.simxGetObjectFloatParameter(clientID,joints[i],2012,vrep.simx_opmode_blocking)
velocity_error[i] = desired_velocity[i] - velocity
#error_collect.append(error)
#print(error)
# while np.sum(np.abs(error))>epsilon:
# for i in range(6):
# if abs(error[i])<epsilon/6.:
# vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
# vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
# else:
# vrep.simxSetJointTargetVelocity(clientID,joints[i],error[i]*9999/abs(error[i]),vrep.simx_opmode_oneshot)
# vrep.simxSetJointForce(clientID,joints[i],abs(Kp*(error[i])),vrep.simx_opmode_oneshot)
# error[i] = desired_angles[i] - (vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
a = []
for i in range(3):
vrep.simxSetJointTargetVelocity(clientID,joints[i],0,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999,vrep.simx_opmode_oneshot)
a.append(vrep.simxGetJointPosition(clientID,joints[i],vrep.simx_opmode_streaming)[1])
# print(a)
else:
for i in range(3):
vrep.simxSetJointTargetVelocity(clientID,joints[i],0.,vrep.simx_opmode_oneshot)
vrep.simxSetJointForce(clientID,joints[i],9999.,vrep.simx_opmode_oneshot)
print('hi')
sub_MTML.unregister()
vrep.simx_opmode_blocking)
if _ !=0 : raise Exception()
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()
# update end-effector position
L = np.array([0, .35]) # segment lengths associated with each joint
s = np.sin(q)
c = np.cos(q)
xyz = np.array([
0,
L[1]*c[0],
L[1]*s[0]])
xyz += np.array([0, 0, 0.425])
track_hand.append(np.copy(xyz)) # store for plotting
def joint_velocity(cid, handle):
err, ret = vrep.simxGetObjectFloatParameter(cid, handle, 2012, vrep_mode)
return [ret]
