def add_robot(self, model):
"""Add new robot to the scene"""
postfix = self.__get_next_postfix()
vrep.simxLoadModel(self.client_id, MODELS_DIR + model, 0xFF,
BLOCKING_MODE)
new_youbot = YouBot(self.client_id, postfix)
self.robots.append(new_youbot)
def get_youbot(self) -> YouBot:
children = self.get_objects_children(
'sim.handle_scene',
children_type='sim.object_shape_type',
filter_children=1 + 2)
for h in children:
name = self.get_object_name(h)
if name == 'youBot':
return YouBot(self, h)
# (C) Copyright Renaud Detry 2013, Thibaut Cuvelier 2017.
# Distributed under the GNU General Public License.
# (See http://www.gnu.org/copyleft/gpl.html)
## Initiate the connection to the simulator.
print('Program started')
VRep.simxFinish(-1)
vrep = VRep('127.0.0.1', 19998, True, True, 2000, 5)
print('Connection %d to the remote API server open.\n' % vrep.clientID)
vrep.simxStopSimulation(simx_opmode_blocking)
vrep.simxStartSimulation(simx_opmode_oneshot_wait)
# Retrieve all handles, mostly the Hokuyo for this example.
youbot = YouBot(vrep)
youbot.streaming_init(vrep)
youbot.hokuyo_init(vrep)
## Read data from the depth camera (Hokuyo)
# Get the position and orientation of the youBot in the world reference frame (as if with a GPS).
youbot_pos = vrep.simxGetObjectPosition(youbot.ref, -1, simx_opmode_buffer)
youbot_euler = vrep.simxGetObjectOrientation(youbot.ref, -1,
simx_opmode_buffer)
# Determine the position of the Hokuyo with global coordinates (world reference frame).
trf = np.asarray(transl(youbot_pos) * trotx(youbot_euler[0]) * troty(youbot_euler[1]) *\
trotz(youbot_euler[2]))
world_hokuyo1 = homtrans(trf, np.asarray(youbot.hokuyo1_pos)[:, None])
world_hokuyo2 = homtrans(trf, np.asarray(youbot.hokuyo2_pos)[:, None])
pass
# This will only work in "continuous remote API server service".
# See http://www.v-rep.eu/helpFiles/en/remoteApiServerSide.htm
vrep.simxStartSimulation(simx_opmode_oneshot_wait)
# Stop the simulation whenever exiting (e.g. ctrl-C)
@atexit.register
def stop_simulation():
vrep.simxStopSimulation(simx_opmode_oneshot_wait)
vrep.simxFinish(vrep.clientID)
# Retrieve all handles, and stream arm and wheel joints, the robot's pose, the Hokuyo, and the
# arm tip pose. The tip corresponds to the point between the two tongs of the gripper (for
# more details, see later or in the file focused/youbot_arm.m).
youbot = YouBot(vrep)
youbot.streaming_init(vrep)
youbot.hokuyo_init(vrep)
# Let a few cycles pass to make sure there's a value waiting for us next time we try to get a
# joint angle or the robot pose with the simx_opmode_buffer option.
sleep(.2)
## Youbot constants
# The time step the simulator is using (your code should run close to it).
timestep = .05
# Minimum and maximum angles for all joints. Only useful to implement custom IK.
arm_joint_ranges = [
-2.9496064186096, 2.9496064186096 - 1.5707963705063,
1.308996796608 - 2.2863812446594, 2.2863812446594 - 1.7802357673645,
def create_youbot(self, x: float, y: float, z: float) -> YouBot:
ix, iy, iz = YouBot.get_position_offsets()
ret = self.create_model('models/robots/mobile/KUKA YouBot.ttm', x + ix,
y + iy, z + iz, 0.)
self.set_object_orientation(ret, *YouBot.get_orientation_offsets())
return YouBot(self, ret)
# Illustrates the V-REP bindings, more specifically the way to take a 3D point cloud.
# (C) Copyright Renaud Detry 2013, Thibaut Cuvelier 2017.
# Distributed under the GNU General Public License.
# (See http://www.gnu.org/copyleft/gpl.html)
## Initiate the connection to the simulator.
print('Program started')
VRep.simxFinish(-1)
vrep = VRep('127.0.0.1', 19998, True, True, 2000, 5)
print('Connection %d to the remote API server open.\n' % vrep.clientID)
vrep.simxStartSimulation(simx_opmode_oneshot_wait)
# Retrieve all handles, mostly the Hokuyo for this example.
youbot = YouBot(vrep)
youbot.hokuyo_init(vrep)
## Read data from the depth camera (Hokuyo)
# Reading a 3D image costs a lot to VREP (it has to simulate the image). It also requires a lot of
# bandwidth, and processing a 3D point cloud (for instance, to find one of the boxes or cylinders
# that the robot has to grasp) will take a long time in MATLAB. In general, you will only want to
# capture a 3D image at specific times, for instance when you believe you're facing one of the
# tables.
# Reduce the view angle to pi/8 in order to better see the objects. Do it only once.
# ^^^^^^ ^^^^^^^^^^ ^^^^ ^^^^^^^^^^^^^^^
# simxSetFloatSignal simx_opmode_oneshot_wait
# |
# rgbd_sensor_scan_angle
# The depth camera has a limited number of rays that gather information. If this number is