├── README.md ├── code ├── controller.py ├── cuboid_collision.py ├── forward_kinematics.py ├── graph.pickle ├── graphs │ ├── graph-01.pickle │ ├── graph-100-10.pickle │ ├── graph-1000-50.pickle │ └── graph-final.pickle ├── lib2 │ ├── __init__.py │ ├── remoteApi.dylib │ ├── vrep.py │ └── vrepConst.py ├── planner.py ├── utilities │ ├── __init__.py │ └── vrep_utils.py └── utils.py ├── handout.pdf ├── plots ├── cuboids │ ├── Cuboid 1 Collision: False .jpg │ ├── Cuboid 2 Collision: False .jpg │ ├── Cuboid 3 Collision: True .jpg │ ├── Cuboid 4 Collision: True .jpg │ ├── Cuboid 5 Collision: True .jpg │ ├── Cuboid 6 Collision: False .jpg │ ├── Cuboid 7 Collision: True .jpg │ └── Cuboid 8 Collision: True .jpg ├── images │ ├── 1000-nodes.png │ ├── Figure_1-1.png │ ├── Figure_1.png │ ├── collision.png │ ├── roadmap-100-10-01-01.png │ ├── roadmap-100-10-01-02.png │ ├── roadmap-100-10-01-03.png │ ├── roadmap-100-10-01-04.png │ └── roadmap.png └── joints │ ├── joint-1.jpg │ ├── joint-2.jpg │ ├── joint-3.jpg │ ├── joint-4.jpg │ ├── joint-5.jpg │ ├── joint-6.jpg │ └── joint-7.jpg ├── scene └── locobot_motion.ttt ├── urdf ├── locobot.dae └── locobot_description_v3.urdf └── writeup └── hvhvalen.pdf /README.md: -------------------------------------------------------------------------------- 1 | # Probabilistic Roadmap Planner 2 | Probabilistic Roadmap Planner with VREP Simulation for LocoBot 3 | 4 | ## Simulation in VREP (YouTube Link) 5 | Simulation in VREP 8 | 9 | ## Cuboid Collision Detection using Seperating Axis Theorem 10 | ![Cuboid Collision Detection using Seperating Axis Theorem](https://github.com/heethesh/Probabilistic-Roadmap-Planner/raw/master/plots/images/collision.png) 11 | 12 | ## Probabilistic Roadmap Planner 13 | #### Generated Roadmap with 100 Nodes 14 | ![Generated Roadmap with 100 Nodes](https://github.com/heethesh/Probabilistic-Roadmap-Planner/raw/master/plots/images/roadmap.png "Generated Roadmap with 100 Nodes") 15 | 16 | #### Generated Roadmap with 1000 Nodes 17 | ![Generated Roadmap with 1000 Nodes](https://github.com/heethesh/Probabilistic-Roadmap-Planner/raw/master/plots/images/1000-nodes.png "Generated Roadmap with 1000 Nodes") 18 | 19 | ## LocoBot Manipulator 20 | #### LocoBot Manipulator with Bounding Boxes 21 | ![LocoBot Manipulator with Bounding Boxes](https://github.com/heethesh/Probabilistic-Roadmap-Planner/raw/master/plots/images/Figure_1-1.png "LocoBot Manipulator with Bounding Boxes") 22 | -------------------------------------------------------------------------------- /code/controller.py: -------------------------------------------------------------------------------- 1 | ''' 2 | 16-662 Robot Autonomy (Spring 2019) 3 | Homework 2 - Motion Planning and Collision Avoidance 4 | Author: Heethesh Vhavle 5 | Email: heethesh@cmu.edu 6 | Version: 1.0.0 7 | ''' 8 | 9 | # Python 2/3 compatibility 10 | from __future__ import print_function 11 | 12 | # Import system libraries 13 | import os 14 | import sys 15 | import time 16 | import argparse 17 | import traceback 18 | 19 | # Modify the following lines if you have problems importing the V-REP utilities 20 | cwd = os.getcwd() 21 | sys.path.append(cwd) 22 | sys.path.append(os.path.join(cwd, 'lib')) 23 | sys.path.append(os.path.join(cwd, 'utilities')) 24 | 25 | # Import application libraries 26 | import numpy as np 27 | import vrep_utils as vu 28 | import matplotlib.pyplot as plt 29 | 30 | ############################################################################### 31 | 32 | class ArmController: 33 | def __init__(self, clientID): 34 | # Do not modify the following variables 35 | self.history = {'timestamp': [], 36 | 'joint_feedback': [], 37 | 'joint_target': [], 38 | 'ctrl_commands': []} 39 | self._target_joint_positions = None 40 | 41 | # Defined variables 42 | self.clientID = clientID 43 | self.first_loop = True 44 | self.wait_time = 2 45 | self.converged_time = 0 46 | self.converged_wait = False 47 | 48 | # PID gains 49 | # self.kp = [4, 5, 4, 4, 4, 4, 4] 50 | self.kp = [3.5, 3.5, 3.5, 3.5, 3.5, 3.5, 3.5] 51 | self.ki = [0.03, 0.1, 0.03, 0.03, 0.03, 0.03, 0.03] 52 | self.kd = [0, 0, 0, 0, 0, 0, 0] 53 | 54 | def constrain(input, min_val, max_val): 55 | if input < min_val: return min_val 56 | elif input > max_val: return max_val 57 | else: return input 58 | 59 | def set_target_joint_positions(self, target_joint_positions): 60 | assert len(target_joint_positions) == vu.N_ARM_JOINTS, \ 61 | 'Expected target joint positions to be length {}, but it was length {} instead.'.format(len(target_joint_positions), vu.N_ARM_JOINTS) 62 | self._target_joint_positions = target_joint_positions 63 | 64 | def calculate_commands_from_feedback(self, timestamp, sensed_joint_positions): 65 | assert self._target_joint_positions is not None, \ 66 | 'Expected target joint positions to be set, but it was not.' 67 | 68 | # Using the input joint feedback, and the known target joint positions, 69 | # calculate the joint commands necessary to drive the system towards 70 | # the target joint positions 71 | ctrl_commands = np.zeros(vu.N_ARM_JOINTS) 72 | 73 | # Input and setpoint 74 | current = np.asarray(sensed_joint_positions) 75 | target = np.asarray(self._target_joint_positions) 76 | 77 | # Error term 78 | error = target - current 79 | # print(error) 80 | 81 | # PID loop 82 | if self.first_loop: 83 | self.first_loop = False 84 | else: 85 | dt = timestamp - self.history['timestamp'][-1] 86 | ctrl_commands = np.multiply(self.kp, error) 87 | ctrl_commands += np.multiply(self.kd, current - np.asarray(self.history['joint_feedback'][-1]) / dt) 88 | ctrl_commands += np.multiply(self.ki, error) 89 | 90 | # Append time history 91 | self.history['timestamp'].append(timestamp) 92 | self.history['joint_feedback'].append(sensed_joint_positions) 93 | self.history['joint_target'].append(self._target_joint_positions) 94 | self.history['ctrl_commands'].append(ctrl_commands) 95 | 96 | return ctrl_commands 97 | 98 | def has_stably_converged_to_target(self): 99 | # Check convergence for last 10 states 100 | converged = np.all(np.isclose(self.history['joint_target'][-1], 101 | self.history['joint_feedback'][-10:], 1e-2, 1e-2)) 102 | 103 | # First converged 104 | if converged and not self.converged_wait: 105 | self.converged_wait = True 106 | self.converged_time = time.time() 107 | return False 108 | 109 | # Wait for seconds before next state 110 | if self.converged_wait and (time.time() - self.converged_time > self.wait_time): 111 | self.converged_wait = False 112 | return True 113 | 114 | return False 115 | 116 | def plot(self, savefile=False): 117 | for i in range(vu.N_ARM_JOINTS): 118 | fig = plt.figure() 119 | plt.plot(self.history['timestamp'], np.array(self.history['joint_feedback'])[:, i]) 120 | plt.plot(self.history['timestamp'], np.array(self.history['joint_target'])[:, i]) 121 | plt.legend(labels=['Joint Sensed', 'Joint Target'], title='Legend', loc=0, fontsize='small', fancybox=True) 122 | plt.title('Time Response for Joint %d' % (i + 1)) 123 | plt.xlabel('Timestamp (s)') 124 | 125 | if i > 4: plt.ylabel('Joint Displacement (m)') 126 | else: plt.ylabel('Joint Angle (rad)') 127 | 128 | if savefile: fig.savefig('joints/joint-%d.jpg' % (i + 1), dpi=480, bbox_inches='tight') 129 | else: plt.show() 130 | 131 | def execute(self, joint_targets, gripper_targets): 132 | # Iterate through target joint positions 133 | for target in np.hstack((joint_targets, gripper_targets)): 134 | # Set new target position 135 | self.set_target_joint_positions(target) 136 | 137 | steady_state_reached = False 138 | while not steady_state_reached: 139 | timestamp = vu.get_sim_time_seconds(self.clientID) 140 | # print('Simulation time: {} sec'.format(timestamp)) 141 | 142 | # Get current joint positions 143 | sensed_joint_positions = vu.get_arm_joint_positions(self.clientID) 144 | 145 | # Calculate commands 146 | commands = self.calculate_commands_from_feedback(timestamp, sensed_joint_positions) 147 | 148 | # Send commands to V-REP 149 | vu.set_arm_joint_target_velocities(self.clientID, commands) 150 | 151 | # Print current joint positions (comment out if you'd like) 152 | # print(sensed_joint_positions) 153 | vu.step_sim(self.clientID, 1) 154 | 155 | # Determine if we've met the condition to move on to the next point 156 | steady_state_reached = self.has_stably_converged_to_target() 157 | 158 | 159 | def main(args): 160 | success = False 161 | try: 162 | # Connect to V-REP 163 | print ('Connecting to V-REP...') 164 | clientID = vu.connect_to_vrep() 165 | print ('Connected.') 166 | 167 | # Reset simulation in case something was running 168 | vu.reset_sim(clientID) 169 | 170 | # Initial control inputs are zero 171 | vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS)) 172 | 173 | # Despite the name, this sets the maximum allowable joint force 174 | vu.set_arm_joint_forces(clientID, 50.*np.ones(vu.N_ARM_JOINTS)) 175 | 176 | # One step to process the above settings 177 | vu.step_sim(clientID) 178 | 179 | # Joint targets, radians for revolute joints and meters for prismatic joints 180 | gripper_targets = np.asarray([[-0.03, 0.03], [-0.03, 0.03]]) 181 | joint_targets = np.radians([[-80, 0, 0, 0, 0], [0, 60, -75, -75, 0]]) 182 | 183 | # Verify fk 184 | from utils import URDFRobot, print_pose 185 | robot = URDFRobot('urdf/locobot_description_v3.urdf') 186 | 187 | from forward_kinematics import ForwardKinematicsSolver 188 | fksolver = ForwardKinematicsSolver(robot) 189 | for target in joint_targets: 190 | print(target) 191 | pose = fksolver.getWristPose(target[:5], robot, vu.ARM_JOINT_NAMES[:5]) 192 | print_pose(pose) 193 | 194 | # Instantiate controller and run it 195 | controller = ArmController(clientID) 196 | controller.execute(joint_targets, gripper_targets) 197 | success = True 198 | 199 | except Exception as e: 200 | print(e) 201 | traceback.print_exc() 202 | 203 | finally: 204 | # Stop VREP simulation 205 | vu.stop_sim(clientID) 206 | 207 | # Plot time histories 208 | if success: controller.plot() 209 | 210 | 211 | if __name__ == "__main__": 212 | parser = argparse.ArgumentParser() 213 | args = parser.parse_args() 214 | main(args) 215 | -------------------------------------------------------------------------------- /code/cuboid_collision.py: -------------------------------------------------------------------------------- 1 | ''' 2 | 16-662 Robot Autonomy (Spring 2019) 3 | Homework 2 - Motion Planning and Collision Avoidance 4 | Author: Heethesh Vhavle 5 | Email: heethesh@cmu.edu 6 | Version: 1.0.0 7 | ''' 8 | 9 | # Python 2/3 compatibility 10 | from __future__ import print_function 11 | 12 | import time 13 | import operator 14 | import itertools 15 | 16 | import numpy as np 17 | import matplotlib.pyplot as plt 18 | from mpl_toolkits.mplot3d import Axes3D 19 | from numpy.core.umath_tests import inner1d 20 | from mpl_toolkits.mplot3d.art3d import Poly3DCollection, Line3DCollection 21 | from tf.transformations import euler_matrix, translation_matrix, quaternion_matrix, rotation_matrix 22 | 23 | 24 | class Cuboid: 25 | def __init__(self, origin, angles, dxyz, name=None, vrep=False): 26 | # Check dimensions 27 | assert len(dxyz) == 3 28 | assert len(origin) == 3 29 | assert len(angles) == 3 or len(angles) == 4 30 | 31 | # Cuboid name 32 | if name: self.name = name 33 | 34 | # Origin 35 | self.origin = np.asarray(origin) 36 | self.origin_matrix = translation_matrix(origin) 37 | 38 | # Orientation 39 | self.angles = np.asarray(angles) 40 | if vrep: 41 | self.rot_matrix = Cuboid.vrep_rotation_matrix(self.angles) 42 | else: 43 | if len(angles) == 3: self.rot_matrix = euler_matrix(angles[0], angles[1], angles[2]) 44 | elif len(angles) == 4: self.rot_matrix = quaternion_matrix(angles) 45 | 46 | # Transformation 47 | self.transform_matrix = np.matmul(self.origin_matrix, self.rot_matrix) 48 | 49 | # Dimensions 50 | self.dxyz = np.asarray(dxyz) 51 | self.xdim = dxyz[0] 52 | self.ydim = dxyz[1] 53 | self.zdim = dxyz[2] 54 | 55 | # Save vertices 56 | self.vertices = self.get_vertices() 57 | 58 | # Optimization 59 | self.rot33 = self.rot_matrix[:3, :3] 60 | self.rot33_opt = np.vstack([self.rot33, self.rot33, self.rot33]) 61 | self.normals = self.rot_matrix[:3, :3].T 62 | self.normals_opt = np.asarray([self.normals[0], self.normals[0], self.normals[0], 63 | self.normals[1], self.normals[1], self.normals[1], 64 | self.normals[2], self.normals[2], self.normals[2]]) 65 | 66 | def get_vertices(self): 67 | ops = list(itertools.product([operator.add, operator.sub], repeat=3)) 68 | vertices = [(op[0](0, self.xdim / 2.0), 69 | op[1](0, self.ydim / 2.0), 70 | op[2](0, self.zdim / 2.0), 1) for op in ops ] 71 | vertices = np.matmul(self.transform_matrix, np.asarray(vertices).T).T[:, :3] 72 | return vertices 73 | 74 | @staticmethod 75 | def vrep_rotation_matrix(rpy): 76 | roll = rotation_matrix(rpy[0], [1, 0, 0]) 77 | pitch = rotation_matrix(rpy[1], [0, 1, 0]) 78 | yaw = rotation_matrix(rpy[2], [0, 0, 1]) 79 | return np.matmul(np.matmul(roll, pitch), yaw) 80 | 81 | 82 | class CollisionChecker: 83 | def __init__(self): 84 | pass 85 | 86 | def get_normals(self, cub1, cub2): 87 | normals = [] 88 | for i in range(3): 89 | # Cube normals (x6) 90 | normals.append(cub1.rot_matrix[:3, i]) 91 | normals.append(cub2.rot_matrix[:3, i]) 92 | 93 | # Normals of normals (x9) 94 | normals += np.array_split((np.cross(cub1.rot_matrix[:3, i], 95 | cub2.rot_matrix[:3, :3]).flatten()), 3) 96 | 97 | return np.asarray(normals) 98 | 99 | def detect_collision(self, cub1, cub2): 100 | # Caluclate all 15 normals 101 | normals = self.get_normals(cub1, cub2) 102 | for normal in normals: 103 | # Calculate projections 104 | projects1 = inner1d(normal, cub1.vertices) 105 | projects2 = inner1d(normal, cub2.vertices) 106 | 107 | # Gap detected 108 | if np.max(projects1) < np.min(projects2) or \ 109 | np.max(projects2) < np.min(projects1): return False 110 | 111 | return True 112 | 113 | def detect_collision_optimized(self, cub1, cub2): 114 | # Caluclate all 15 normals 115 | normals = np.zeros((15, 3)) 116 | normals[0:3, :] = cub1.normals 117 | normals[3:6, :] = cub2.normals 118 | normals[6:15, :] = np.cross(cub1.normals_opt, cub2.rot33_opt) 119 | 120 | for normal in normals: 121 | # Calculate projections 122 | projects1 = inner1d(normal, cub1.vertices) 123 | projects2 = inner1d(normal, cub2.vertices) 124 | 125 | # Gap detected - no collision 126 | if np.max(projects1) < np.min(projects2) or \ 127 | np.max(projects2) < np.min(projects1): return False 128 | 129 | # Cuboids in collision 130 | return True 131 | 132 | @staticmethod 133 | def display_cuboids(cuboids, title=None, savefile=None, return_ax=False): 134 | fig = plt.figure() 135 | ax = fig.add_subplot(111, projection='3d') 136 | ax.set_aspect('equal') 137 | 138 | # Equalize display aspect ratio for all axes 139 | points = np.asarray([0, 0, 0]) 140 | for cuboid in cuboids: points = np.vstack((points, cuboid.vertices)) 141 | 142 | max_range = (np.array([points[:, 0].max() - points[:, 0].min(), 143 | points[:, 1].max() - points[:, 1].min(), 144 | points[:, 2].max() - points[:, 2].min()]).max() / 2.0) 145 | 146 | mid_x = (points[:, 0].max() + points[:, 0].min()) * 0.5 147 | mid_y = (points[:, 1].max() + points[:, 1].min()) * 0.5 148 | mid_z = (points[:, 2].max() + points[:, 2].min()) * 0.5 149 | 150 | ax.set_xlim(mid_x - max_range, mid_x + max_range) 151 | ax.set_ylim(mid_y - max_range, mid_y + max_range) 152 | ax.set_zlim(mid_z - max_range, mid_z + max_range) 153 | 154 | # Compute edges 155 | edges = lambda x: [[x[0], x[1], x[3], x[2]], [x[4], x[5], x[7], x[6]], 156 | [x[0], x[1], x[5], x[4]], [x[2], x[3], x[7], x[6]], 157 | [x[0], x[2], x[6], x[4]], [x[5], x[7], x[3], x[1]]] 158 | 159 | # Draw faces 160 | colors = plt.get_cmap('tab10') 161 | for i, cuboid in enumerate(cuboids): 162 | ax.add_collection3d(Poly3DCollection(edges(cuboid.vertices), linewidths=2, edgecolors='k', alpha=0.5, facecolor=colors(i % 10))) 163 | ax.scatter(cuboid.vertices[:, 0], cuboid.vertices[:, 1], cuboid.vertices[:, 2], c='r') 164 | 165 | ax.set_xlabel('X-Axis') 166 | ax.set_ylabel('Y-Axis') 167 | ax.set_zlabel('Z-Axis') 168 | 169 | if return_ax: return ax 170 | if title: plt.title(title) 171 | if savefile: fig.savefig('%s.jpg' % savefile, dpi=480, bbox_inches='tight') 172 | else: plt.show() 173 | 174 | 175 | def run_test_cases(): 176 | # Collision checker 177 | collision_checker = CollisionChecker() 178 | 179 | # Reference cuboid 180 | cuboid_ref = Cuboid([0, 0, 0], [0, 0, 0], [3, 1, 2]) 181 | 182 | # Test cuboids 183 | test_cuboids = [Cuboid([0, 1, 0], [0, 0, 0], [0.8, 0.8, 0.8]), 184 | Cuboid([1.5, -1.5, 0], [1, 0, 1.5], [1, 3, 3]), 185 | Cuboid([0, 0, -1], [0, 0, 0], [2, 3, 1]), 186 | Cuboid([3, 0, 0], [0, 0, 0], [3, 1, 1]), 187 | Cuboid([-1, 0, -2], [.5, 0, 0.4], [2, 0.7, 2]), 188 | Cuboid([1.8, 0.5, 1.5], [-0.2, 0.5, 0], [1, 3, 1]), 189 | Cuboid([0, -1.2, 0.4], [0, 0.785, 0.785], [1, 1, 1]), 190 | Cuboid([-0.8, 0, -0.5], [0, 0, 0.2], [1, 0.5, 0.5])] 191 | 192 | # Check collisions 193 | for i, test_cuboid in enumerate(test_cuboids): 194 | ret = collision_checker.detect_collision_optimized(cuboid_ref, test_cuboid) 195 | print('Cuboid %d Collision:' % (i + 1), ret) 196 | collision_checker.display_cuboids([cuboid_ref, test_cuboid], 197 | title='Cuboid %d Collision: %s\n' % (i + 1, ret)) 198 | 199 | # Time check 200 | start_time = time.time() 201 | for test_cuboid in test_cuboids[:3] + test_cuboids * 4: 202 | ret = collision_checker.detect_collision_optimized(cuboid_ref, test_cuboid) 203 | print('35 Checks Took:', time.time() - start_time) 204 | 205 | if __name__ == '__main__': 206 | run_test_cases() 207 | -------------------------------------------------------------------------------- /code/forward_kinematics.py: -------------------------------------------------------------------------------- 1 | ''' 2 | 16-662 Robot Autonomy (Spring 2019) 3 | Homework 2 - Motion Planning and Collision Avoidance 4 | Author: Heethesh Vhavle 5 | Email: heethesh@cmu.edu 6 | Version: 1.0.0 7 | 8 | The order of the targets are as follows: 9 | joint_1 / revolute / arm_base_link <- shoulder_link 10 | joint_2 / revolute / shoulder_link <- elbow_link 11 | joint_3 / revolute / elbow_link <- forearm_link 12 | joint_4 / revolute / forearm_link <- wrist_link 13 | joint_5 / revolute / wrist_link <- gripper_link 14 | joint_6 / prismatic / gripper_link <- finger_r 15 | joint_7 / prismatic / gripper_link <- finger_l 16 | ''' 17 | 18 | # Python 2/3 compatibility 19 | from __future__ import print_function 20 | 21 | import os 22 | import argparse 23 | 24 | import numpy as np 25 | from tf.transformations import euler_matrix, rotation_matrix, translation_matrix, decompose_matrix 26 | 27 | from cuboid_collision import Cuboid 28 | from utils import URDFRobot, print_pose, JOINT_NAMES 29 | 30 | 31 | class Joint(object): 32 | def __init__(self, origin, angles, axis, name=None, vrep=False): 33 | # Check dimensions 34 | assert len(axis) == 3 35 | assert len(origin) == 3 36 | assert len(angles) == 3 or len(angles) == 4 37 | 38 | # Joint name 39 | if name: self.name = name 40 | 41 | # Origin 42 | self.origin = np.asarray(origin) 43 | self.origin_matrix = translation_matrix(origin) 44 | 45 | # Orientation 46 | self.angles = np.asarray(angles) 47 | if vrep: 48 | self.rot_matrix = Cuboid.vrep_rotation_matrix(self.angles) 49 | else: 50 | if len(angles) == 3: self.rot_matrix = euler_matrix(angles[0], angles[1], angles[2]) 51 | elif len(angles) == 4: self.rot_matrix = quaternion_matrix(angles) 52 | 53 | # Joint axis 54 | self.axis = axis 55 | 56 | 57 | class PrismaticJoint(Joint): 58 | def __init__(self, origin, angles, axis, offset=0, name=None): 59 | # Init other parameters 60 | super(PrismaticJoint, self).__init__(origin, angles, axis, name) 61 | self.set_joint_offset(offset) 62 | 63 | def set_joint_offset(self, offset): 64 | # Prismatic joint properties 65 | self.offset = offset 66 | self.offset_xyz = np.full_like(self.axis, self.offset) * self.axis 67 | self.offset_matrix = translation_matrix(self.offset_xyz) 68 | 69 | 70 | class RevoluteJoint(Joint): 71 | def __init__(self, origin, angles, axis, offset=0, name=None): 72 | # Init other parameters 73 | super(RevoluteJoint, self).__init__(origin, angles, axis, name) 74 | self.set_joint_offset(offset) 75 | 76 | def set_joint_offset(self, offset): 77 | # Revolute joint properties 78 | self.offset = offset 79 | self.offset_matrix = rotation_matrix(self.offset, self.axis) 80 | 81 | 82 | class ForwardKinematicsSolver: 83 | def __init__(self, robot): 84 | # URDF object 85 | self.robot = robot 86 | 87 | # Joint objects 88 | self.joints = [] 89 | 90 | # Link to joint transforms 91 | self.link_to_joints = [] 92 | 93 | def update_joints(self, joint_angles, joint_poses=None): 94 | # print('>>> Joint Angles:', np.degrees(joint_angles)) 95 | # Check dimensions 96 | 97 | if self.joints: 98 | for joint, offset in zip(self.joints, joint_angles): 99 | joint.set_joint_offset(offset) 100 | else: 101 | assert len(joint_angles) == len(joint_poses) 102 | for i, joint_name in enumerate(JOINT_NAMES): 103 | # Get joint axis from URDF 104 | axis = self.robot.get_joint_axis(joint_name) 105 | 106 | # Select joint type 107 | if i < 5: JointType = RevoluteJoint 108 | else: JointType = PrismaticJoint 109 | 110 | # Create joint object 111 | self.joints.append(JointType(origin=joint_poses[i][0], angles=joint_poses[i][1], 112 | axis=axis, offset=joint_angles[i], name=joint_name)) 113 | 114 | def compute(self, links, joint_angles, joint_poses=None, setup=False): 115 | # Check dimensions 116 | assert len(joint_angles) >= 4 117 | 118 | # Update joint angles 119 | self.update_joints(joint_angles, joint_poses) 120 | 121 | # Links and joint poses 122 | pose = np.eye(4) 123 | link_cuboids = links[:1] 124 | 125 | # Compute forward kinematics 126 | for i, joint in enumerate(self.joints): 127 | # Compute joint pose 128 | R, T, axis = self.robot.get_joint_pose(joint.name) 129 | pose = np.matmul(pose, np.matmul(np.matmul(R, T), joint.offset_matrix)) 130 | 131 | # Update arm joints 132 | if i < 5: 133 | wrist_pose = pose 134 | 135 | # Setup link to joint transforms 136 | if setup: 137 | self.link_to_joints.append(np.matmul(np.linalg.inv(wrist_pose), links[i + 1].transform_matrix)) 138 | 139 | # Compute link pose 140 | else: 141 | link_pose = np.matmul(wrist_pose, self.link_to_joints[i]) 142 | _, _, R, T, _ = decompose_matrix(link_pose) 143 | 144 | # Create link cuboids 145 | link_cuboids.append(Cuboid(T, R, links[i + 1].dxyz, links[i + 1].name)) 146 | 147 | # Return wrist pose and link cuboids 148 | _, _, R, T, _ = decompose_matrix(wrist_pose) 149 | return R, T, link_cuboids 150 | 151 | # Backward compatible 152 | @staticmethod 153 | def getWristPose(joint_angle_list, robot, joint_names, intermediate_pose=False): 154 | ''' 155 | Get the wrist pose for given joint angle configuration. 156 | 157 | joint_angle_list: List of joint angles to get final wrist pose for 158 | kwargs: Other keyword arguments use as required. 159 | 160 | Return: List of 16 values which represent the joint wrist pose 161 | obtained from the End-Effector Transformation matrix using column-major 162 | ordering. 163 | ''' 164 | 165 | pose = np.eye(4) 166 | intermediate_poses = [] 167 | 168 | for joint, angle in zip(joint_names, joint_angle_list): 169 | R, T, axis = robot.get_joint_pose(joint) 170 | A = rotation_matrix(angle, axis) 171 | pose = np.matmul(np.matmul(pose, np.matmul(R, T)), A) 172 | intermediate_poses.append((pose, axis)) 173 | 174 | if intermediate_pose: 175 | return intermediate_poses 176 | else: 177 | return pose 178 | 179 | # Backward compatible 180 | @staticmethod 181 | def getWristJacobian(joint_angle_list, robot, joint_names): 182 | ''' 183 | Get the wrist Jacobian for given joint angle configuration. 184 | 185 | joint_angle_list: List of joint angles to get final wrist pose for 186 | kwargs: Other keyword arguments use as required. 187 | 188 | Return: List of 16 values which represent the joint wrist pose 189 | obtained from the End-Effector Transformation matrix using column-major 190 | ordering. 191 | ''' 192 | jacobian = np.zeros((6, 5)) 193 | joint_angles = np.asarray(joint_angle_list) 194 | 195 | poses = getWristPose(joint_angle_list, robot, joint_names, intermediate_pose=True) 196 | for i, (pose, axis) in enumerate(poses): 197 | jacobian[3:6, i] = np.matmul(pose[:3, :3], axis) 198 | jacobian[0:3, i] = np.cross(jacobian[3:6, i].T, poses[-1][0][:3, -1] - pose[:3, -1]) 199 | 200 | return jacobian 201 | 202 | 203 | def main(args): 204 | # Joint angles 205 | joint_angles = args.joints 206 | assert len(joint_angles) == 5, 'Incorrect number of joints specified.' 207 | 208 | # Joint names 209 | joint_names = JOINT_NAMES[:-2] 210 | 211 | # Parse URDF XML 212 | robot = URDFRobot('urdf/locobot_description_v3.urdf') 213 | 214 | # Compute end effector pose and Jacobian 215 | pose = ForwardKinematicsSolver.getWristPose(joint_angles, robot, joint_names) 216 | jacobian = ForwardKinematicsSolver.getWristJacobian(joint_angles, robot, joint_names) 217 | 218 | print('Wrist Pose:\n{}'.format(np.array_str(np.array(pose), precision=3))) 219 | print('Jacobian:\n{}'.format(np.array_str(np.array(jacobian), precision=3))) 220 | 221 | 222 | def test(): 223 | # Joint info 224 | joint_names = JOINT_NAMES[:-2] 225 | joint_angles = np.radians([-80, 0, 0, 0, 0]) 226 | 227 | # Parse URDF XML 228 | robot = URDFRobot('urdf/locobot_description_v3.urdf') 229 | 230 | # Compute end effector pose and Jacobian 231 | poses = ForwardKinematicsSolver.getWristPose(joint_angles, robot, joint_names, intermediate_pose=True) 232 | for i, (pose, axis) in enumerate(poses): print_pose(pose) 233 | 234 | 235 | if __name__ == '__main__': 236 | # parser = argparse.ArgumentParser( 237 | # description='Get wrist pose using forward kinematics') 238 | # parser.add_argument('--joints', type=float, nargs='+', 239 | # required=True, help='Joint angles to get wrist position for.') 240 | # args = parser.parse_args() 241 | 242 | # main(args) 243 | test() 244 | -------------------------------------------------------------------------------- /code/lib2/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/code/lib2/__init__.py -------------------------------------------------------------------------------- /code/lib2/remoteApi.dylib: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/code/lib2/remoteApi.dylib -------------------------------------------------------------------------------- /code/lib2/vrep.py: -------------------------------------------------------------------------------- 1 | import platform 2 | import struct 3 | import sys 4 | import os 5 | import ctypes as ct 6 | from vrepConst import * 7 | 8 | #load library 9 | libsimx = None 10 | try: 11 | file_extension = '.so' 12 | if platform.system() =='cli': 13 | file_extension = '.dll' 14 | elif platform.system() =='Windows': 15 | file_extension = '.dll' 16 | elif platform.system() == 'Darwin': 17 | file_extension = '.dylib' 18 | else: 19 | file_extension = '.so' 20 | libfullpath = os.path.join(os.path.dirname(__file__), 'remoteApi' + file_extension) 21 | libsimx = ct.CDLL(libfullpath) 22 | except: 23 | print ('----------------------------------------------------') 24 | print ('The remoteApi library could not be loaded. Make sure') 25 | print ('it is located in the same folder as "vrep.py", or') 26 | print ('appropriately adjust the file "vrep.py"') 27 | print ('----------------------------------------------------') 28 | print ('') 29 | 30 | #ctypes wrapper prototypes 31 | c_GetJointPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointPosition", libsimx)) 32 | c_SetJointPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointPosition", libsimx)) 33 | c_GetJointMatrix = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointMatrix", libsimx)) 34 | c_SetSphericalJointMatrix = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetSphericalJointMatrix", libsimx)) 35 | c_SetJointTargetVelocity = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointTargetVelocity", libsimx)) 36 | c_SetJointTargetPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointTargetPosition", libsimx)) 37 | c_GetJointForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetJointForce", libsimx)) 38 | c_SetJointForce = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetJointForce", libsimx)) 39 | c_ReadForceSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.c_int32)(("simxReadForceSensor", libsimx)) 40 | c_BreakForceSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxBreakForceSensor", libsimx)) 41 | c_ReadVisionSensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.POINTER(ct.c_float)), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxReadVisionSensor", libsimx)) 42 | c_GetObjectHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectHandle", libsimx)) 43 | c_GetVisionSensorImage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_byte)), ct.c_ubyte, ct.c_int32)(("simxGetVisionSensorImage", libsimx)) 44 | c_SetVisionSensorImage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_byte), ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetVisionSensorImage", libsimx)) 45 | c_GetVisionSensorDepthBuffer= ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)), ct.c_int32)(("simxGetVisionSensorDepthBuffer", libsimx)) 46 | c_GetObjectChild = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectChild", libsimx)) 47 | c_GetObjectParent = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectParent", libsimx)) 48 | c_ReadProximitySensor = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_float), ct.c_int32)(("simxReadProximitySensor", libsimx)) 49 | c_LoadModel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.POINTER(ct.c_int32), ct.c_int32)(("simxLoadModel", libsimx)) 50 | c_LoadUI = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxLoadUI", libsimx)) 51 | c_LoadScene = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_ubyte, ct.c_int32)(("simxLoadScene", libsimx)) 52 | c_StartSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxStartSimulation", libsimx)) 53 | c_PauseSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxPauseSimulation", libsimx)) 54 | c_StopSimulation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxStopSimulation", libsimx)) 55 | c_GetUIHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIHandle", libsimx)) 56 | c_GetUISlider = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUISlider", libsimx)) 57 | c_SetUISlider = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetUISlider", libsimx)) 58 | c_GetUIEventButton = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIEventButton", libsimx)) 59 | c_GetUIButtonProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetUIButtonProperty", libsimx)) 60 | c_SetUIButtonProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetUIButtonProperty", libsimx)) 61 | c_AddStatusbarMessage = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxAddStatusbarMessage", libsimx)) 62 | c_AuxiliaryConsoleOpen = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.c_int32)(("simxAuxiliaryConsoleOpen", libsimx)) 63 | c_AuxiliaryConsoleClose = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxAuxiliaryConsoleClose", libsimx)) 64 | c_AuxiliaryConsolePrint = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxAuxiliaryConsolePrint", libsimx)) 65 | c_AuxiliaryConsoleShow = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxAuxiliaryConsoleShow", libsimx)) 66 | c_GetObjectOrientation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectOrientation", libsimx)) 67 | c_GetObjectQuaternion = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectQuaternion", libsimx)) 68 | c_GetObjectPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectPosition", libsimx)) 69 | c_SetObjectOrientation = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectOrientation", libsimx)) 70 | c_SetObjectQuaternion = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectQuaternion", libsimx)) 71 | c_SetObjectPosition = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetObjectPosition", libsimx)) 72 | c_SetObjectParent = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetObjectParent", libsimx)) 73 | c_SetUIButtonLabel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32)(("simxSetUIButtonLabel", libsimx)) 74 | c_GetLastErrors = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetLastErrors", libsimx)) 75 | c_GetArrayParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetArrayParameter", libsimx)) 76 | c_SetArrayParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxSetArrayParameter", libsimx)) 77 | c_GetBooleanParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxGetBooleanParameter", libsimx)) 78 | c_SetBooleanParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_ubyte, ct.c_int32)(("simxSetBooleanParameter", libsimx)) 79 | c_GetIntegerParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetIntegerParameter", libsimx)) 80 | c_SetIntegerParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetIntegerParameter", libsimx)) 81 | c_GetFloatingParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetFloatingParameter", libsimx)) 82 | c_SetFloatingParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetFloatingParameter", libsimx)) 83 | c_GetStringParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetStringParameter", libsimx)) 84 | c_GetCollisionHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetCollisionHandle", libsimx)) 85 | c_GetDistanceHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetDistanceHandle", libsimx)) 86 | c_GetCollectionHandle = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetCollectionHandle", libsimx)) 87 | c_ReadCollision = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxReadCollision", libsimx)) 88 | c_ReadDistance = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxReadDistance", libsimx)) 89 | c_RemoveObject = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveObject", libsimx)) 90 | c_RemoveModel = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveModel", libsimx)) 91 | c_RemoveUI = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxRemoveUI", libsimx)) 92 | c_CloseScene = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32)(("simxCloseScene", libsimx)) 93 | c_GetObjects = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.c_int32)(("simxGetObjects", libsimx)) 94 | c_DisplayDialog = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.POINTER(ct.c_int32), ct.POINTER(ct.c_int32), ct.c_int32)(("simxDisplayDialog", libsimx)) 95 | c_EndDialog = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32)(("simxEndDialog", libsimx)) 96 | c_GetDialogInput = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetDialogInput", libsimx)) 97 | c_GetDialogResult = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetDialogResult", libsimx)) 98 | c_CopyPasteObjects = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32, ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxCopyPasteObjects", libsimx)) 99 | c_GetObjectSelection = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectSelection", libsimx)) 100 | c_SetObjectSelection = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32, ct.c_int32)(("simxSetObjectSelection", libsimx)) 101 | c_ClearFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearFloatSignal", libsimx)) 102 | c_ClearIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearIntegerSignal", libsimx)) 103 | c_ClearStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxClearStringSignal", libsimx)) 104 | c_GetFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_float), ct.c_int32)(("simxGetFloatSignal", libsimx)) 105 | c_GetIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetIntegerSignal", libsimx)) 106 | c_GetStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetStringSignal", libsimx)) 107 | c_SetFloatSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_float, ct.c_int32)(("simxSetFloatSignal", libsimx)) 108 | c_SetIntegerSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32)(("simxSetIntegerSignal", libsimx)) 109 | c_SetStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxSetStringSignal", libsimx)) 110 | c_AppendStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxAppendStringSignal", libsimx)) 111 | c_WriteStringStream = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.c_int32)(("simxWriteStringStream", libsimx)) 112 | c_GetObjectFloatParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectFloatParameter", libsimx)) 113 | c_SetObjectFloatParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_float, ct.c_int32)(("simxSetObjectFloatParameter", libsimx)) 114 | c_GetObjectIntParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetObjectIntParameter", libsimx)) 115 | c_SetObjectIntParameter = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetObjectIntParameter", libsimx)) 116 | c_GetModelProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetModelProperty", libsimx)) 117 | c_SetModelProperty = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.c_int32)(("simxSetModelProperty", libsimx)) 118 | c_Start = ct.CFUNCTYPE(ct.c_int32,ct.POINTER(ct.c_char), ct.c_int32, ct.c_ubyte, ct.c_ubyte, ct.c_int32, ct.c_int32)(("simxStart", libsimx)) 119 | c_Finish = ct.CFUNCTYPE(None, ct.c_int32)(("simxFinish", libsimx)) 120 | c_GetPingTime = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetPingTime", libsimx)) 121 | c_GetLastCmdTime = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxGetLastCmdTime", libsimx)) 122 | c_SynchronousTrigger = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxSynchronousTrigger", libsimx)) 123 | c_Synchronous = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_ubyte)(("simxSynchronous", libsimx)) 124 | c_PauseCommunication = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_ubyte)(("simxPauseCommunication", libsimx)) 125 | c_GetInMessageInfo = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetInMessageInfo", libsimx)) 126 | c_GetOutMessageInfo = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32))(("simxGetOutMessageInfo", libsimx)) 127 | c_GetConnectionId = ct.CFUNCTYPE(ct.c_int32,ct.c_int32)(("simxGetConnectionId", libsimx)) 128 | c_CreateBuffer = ct.CFUNCTYPE(ct.POINTER(ct.c_ubyte), ct.c_int32)(("simxCreateBuffer", libsimx)) 129 | c_ReleaseBuffer = ct.CFUNCTYPE(None, ct.c_void_p)(("simxReleaseBuffer", libsimx)) 130 | c_TransferFile = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_char), ct.c_int32, ct.c_int32)(("simxTransferFile", libsimx)) 131 | c_EraseFile = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.c_int32)(("simxEraseFile", libsimx)) 132 | c_GetAndClearStringSignal = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxGetAndClearStringSignal", libsimx)) 133 | c_ReadStringStream = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxReadStringStream", libsimx)) 134 | c_CreateDummy = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_float, ct.POINTER(ct.c_ubyte), ct.POINTER(ct.c_int32), ct.c_int32)(("simxCreateDummy", libsimx)) 135 | c_Query = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.c_ubyte), ct.c_int32, ct.POINTER(ct.c_char), ct.POINTER(ct.POINTER(ct.c_ubyte)), ct.POINTER(ct.c_int32), ct.c_int32)(("simxQuery", libsimx)) 136 | c_GetObjectGroupData = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.c_int32, ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)), ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)), ct.c_int32)(("simxGetObjectGroupData", libsimx)) 137 | c_GetObjectVelocity = ct.CFUNCTYPE(ct.c_int32,ct.c_int32, ct.c_int32, ct.POINTER(ct.c_float), ct.POINTER(ct.c_float), ct.c_int32)(("simxGetObjectVelocity", libsimx)) 138 | c_CallScriptFunction = ct.CFUNCTYPE(ct.c_int32,ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_int32),ct.c_int32,ct.POINTER(ct.c_float),ct.c_int32,ct.POINTER(ct.c_char),ct.c_int32,ct.POINTER(ct.c_ubyte),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_int32)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_float)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_char)),ct.POINTER(ct.c_int32), ct.POINTER(ct.POINTER(ct.c_ubyte)),ct.c_int32)(("simxCallScriptFunction", libsimx)) 139 | 140 | #API functions 141 | def simxGetJointPosition(clientID, jointHandle, operationMode): 142 | ''' 143 | Please have a look at the function description/documentation in the V-REP user manual 144 | ''' 145 | position = ct.c_float() 146 | return c_GetJointPosition(clientID, jointHandle, ct.byref(position), operationMode), position.value 147 | 148 | def simxSetJointPosition(clientID, jointHandle, position, operationMode): 149 | ''' 150 | Please have a look at the function description/documentation in the V-REP user manual 151 | ''' 152 | 153 | return c_SetJointPosition(clientID, jointHandle, position, operationMode) 154 | 155 | def simxGetJointMatrix(clientID, jointHandle, operationMode): 156 | ''' 157 | Please have a look at the function description/documentation in the V-REP user manual 158 | ''' 159 | matrix = (ct.c_float*12)() 160 | ret = c_GetJointMatrix(clientID, jointHandle, matrix, operationMode) 161 | arr = [] 162 | for i in range(12): 163 | arr.append(matrix[i]) 164 | return ret, arr 165 | 166 | def simxSetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode): 167 | ''' 168 | Please have a look at the function description/documentation in the V-REP user manual 169 | ''' 170 | matrix = (ct.c_float*12)(*matrix) 171 | return c_SetSphericalJointMatrix(clientID, jointHandle, matrix, operationMode) 172 | 173 | def simxSetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode): 174 | ''' 175 | Please have a look at the function description/documentation in the V-REP user manual 176 | ''' 177 | 178 | return c_SetJointTargetVelocity(clientID, jointHandle, targetVelocity, operationMode) 179 | 180 | def simxSetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode): 181 | ''' 182 | Please have a look at the function description/documentation in the V-REP user manual 183 | ''' 184 | 185 | return c_SetJointTargetPosition(clientID, jointHandle, targetPosition, operationMode) 186 | 187 | def simxJointGetForce(clientID, jointHandle, operationMode): 188 | ''' 189 | Please have a look at the function description/documentation in the V-REP user manual 190 | ''' 191 | force = ct.c_float() 192 | return c_GetJointForce(clientID, jointHandle, ct.byref(force), operationMode), force.value 193 | 194 | def simxGetJointForce(clientID, jointHandle, operationMode): 195 | ''' 196 | Please have a look at the function description/documentation in the V-REP user manual 197 | ''' 198 | force = ct.c_float() 199 | return c_GetJointForce(clientID, jointHandle, ct.byref(force), operationMode), force.value 200 | 201 | def simxSetJointForce(clientID, jointHandle, force, operationMode): 202 | ''' 203 | Please have a look at the function description/documentation in the V-REP user manual 204 | ''' 205 | return c_SetJointForce(clientID, jointHandle, force, operationMode) 206 | 207 | def simxReadForceSensor(clientID, forceSensorHandle, operationMode): 208 | ''' 209 | Please have a look at the function description/documentation in the V-REP user manual 210 | ''' 211 | state = ct.c_ubyte() 212 | forceVector = (ct.c_float*3)() 213 | torqueVector = (ct.c_float*3)() 214 | ret = c_ReadForceSensor(clientID, forceSensorHandle, ct.byref(state), forceVector, torqueVector, operationMode) 215 | arr1 = [] 216 | for i in range(3): 217 | arr1.append(forceVector[i]) 218 | arr2 = [] 219 | for i in range(3): 220 | arr2.append(torqueVector[i]) 221 | #if sys.version_info[0] == 3: 222 | # state=state.value 223 | #else: 224 | # state=ord(state.value) 225 | return ret, state.value, arr1, arr2 226 | 227 | def simxBreakForceSensor(clientID, forceSensorHandle, operationMode): 228 | ''' 229 | Please have a look at the function description/documentation in the V-REP user manual 230 | ''' 231 | return c_BreakForceSensor(clientID, forceSensorHandle, operationMode) 232 | 233 | def simxReadVisionSensor(clientID, sensorHandle, operationMode): 234 | ''' 235 | Please have a look at the function description/documentation in the V-REP user manual 236 | ''' 237 | 238 | detectionState = ct.c_ubyte() 239 | auxValues = ct.POINTER(ct.c_float)() 240 | auxValuesCount = ct.POINTER(ct.c_int)() 241 | ret = c_ReadVisionSensor(clientID, sensorHandle, ct.byref(detectionState), ct.byref(auxValues), ct.byref(auxValuesCount), operationMode) 242 | 243 | auxValues2 = [] 244 | if ret == 0: 245 | s = 0 246 | for i in range(auxValuesCount[0]): 247 | auxValues2.append(auxValues[s:s+auxValuesCount[i+1]]) 248 | s += auxValuesCount[i+1] 249 | 250 | #free C buffers 251 | c_ReleaseBuffer(auxValues) 252 | c_ReleaseBuffer(auxValuesCount) 253 | 254 | return ret, bool(detectionState.value!=0), auxValues2 255 | 256 | def simxGetObjectHandle(clientID, objectName, operationMode): 257 | ''' 258 | Please have a look at the function description/documentation in the V-REP user manual 259 | ''' 260 | handle = ct.c_int() 261 | if (sys.version_info[0] == 3) and (type(objectName) is str): 262 | objectName=objectName.encode('utf-8') 263 | return c_GetObjectHandle(clientID, objectName, ct.byref(handle), operationMode), handle.value 264 | 265 | def simxGetVisionSensorImage(clientID, sensorHandle, options, operationMode): 266 | ''' 267 | Please have a look at the function description/documentation in the V-REP user manual 268 | ''' 269 | 270 | resolution = (ct.c_int*2)() 271 | c_image = ct.POINTER(ct.c_byte)() 272 | bytesPerPixel = 3 273 | if (options and 1) != 0: 274 | bytesPerPixel = 1 275 | ret = c_GetVisionSensorImage(clientID, sensorHandle, resolution, ct.byref(c_image), options, operationMode) 276 | 277 | reso = [] 278 | image = [] 279 | if (ret == 0): 280 | image = [None]*resolution[0]*resolution[1]*bytesPerPixel 281 | for i in range(resolution[0] * resolution[1] * bytesPerPixel): 282 | image[i] = c_image[i] 283 | for i in range(2): 284 | reso.append(resolution[i]) 285 | return ret, reso, image 286 | 287 | def simxSetVisionSensorImage(clientID, sensorHandle, image, options, operationMode): 288 | ''' 289 | Please have a look at the function description/documentation in the V-REP user manual 290 | ''' 291 | size = len(image) 292 | image_bytes = (ct.c_byte*size)(*image) 293 | return c_SetVisionSensorImage(clientID, sensorHandle, image_bytes, size, options, operationMode) 294 | 295 | def simxGetVisionSensorDepthBuffer(clientID, sensorHandle, operationMode): 296 | ''' 297 | Please have a look at the function description/documentation in the V-REP user manual 298 | ''' 299 | c_buffer = ct.POINTER(ct.c_float)() 300 | resolution = (ct.c_int*2)() 301 | ret = c_GetVisionSensorDepthBuffer(clientID, sensorHandle, resolution, ct.byref(c_buffer), operationMode) 302 | reso = [] 303 | buffer = [] 304 | if (ret == 0): 305 | buffer = [None]*resolution[0]*resolution[1] 306 | for i in range(resolution[0] * resolution[1]): 307 | buffer[i] = c_buffer[i] 308 | for i in range(2): 309 | reso.append(resolution[i]) 310 | return ret, reso, buffer 311 | 312 | def simxGetObjectChild(clientID, parentObjectHandle, childIndex, operationMode): 313 | ''' 314 | Please have a look at the function description/documentation in the V-REP user manual 315 | ''' 316 | childObjectHandle = ct.c_int() 317 | return c_GetObjectChild(clientID, parentObjectHandle, childIndex, ct.byref(childObjectHandle), operationMode), childObjectHandle.value 318 | 319 | def simxGetObjectParent(clientID, childObjectHandle, operationMode): 320 | ''' 321 | Please have a look at the function description/documentation in the V-REP user manual 322 | ''' 323 | 324 | parentObjectHandle = ct.c_int() 325 | return c_GetObjectParent(clientID, childObjectHandle, ct.byref(parentObjectHandle), operationMode), parentObjectHandle.value 326 | 327 | def simxReadProximitySensor(clientID, sensorHandle, operationMode): 328 | ''' 329 | Please have a look at the function description/documentation in the V-REP user manual 330 | ''' 331 | 332 | detectionState = ct.c_ubyte() 333 | detectedObjectHandle = ct.c_int() 334 | detectedPoint = (ct.c_float*3)() 335 | detectedSurfaceNormalVector = (ct.c_float*3)() 336 | ret = c_ReadProximitySensor(clientID, sensorHandle, ct.byref(detectionState), detectedPoint, ct.byref(detectedObjectHandle), detectedSurfaceNormalVector, operationMode) 337 | arr1 = [] 338 | for i in range(3): 339 | arr1.append(detectedPoint[i]) 340 | arr2 = [] 341 | for i in range(3): 342 | arr2.append(detectedSurfaceNormalVector[i]) 343 | return ret, bool(detectionState.value!=0), arr1, detectedObjectHandle.value, arr2 344 | 345 | def simxLoadModel(clientID, modelPathAndName, options, operationMode): 346 | ''' 347 | Please have a look at the function description/documentation in the V-REP user manual 348 | ''' 349 | baseHandle = ct.c_int() 350 | if (sys.version_info[0] == 3) and (type(modelPathAndName) is str): 351 | modelPathAndName=modelPathAndName.encode('utf-8') 352 | return c_LoadModel(clientID, modelPathAndName, options, ct.byref(baseHandle), operationMode), baseHandle.value 353 | 354 | def simxLoadUI(clientID, uiPathAndName, options, operationMode): 355 | ''' 356 | Please have a look at the function description/documentation in the V-REP user manual 357 | ''' 358 | 359 | count = ct.c_int() 360 | uiHandles = ct.POINTER(ct.c_int)() 361 | if (sys.version_info[0] == 3) and (type(uiPathAndName) is str): 362 | uiPathAndName=uiPathAndName.encode('utf-8') 363 | ret = c_LoadUI(clientID, uiPathAndName, options, ct.byref(count), ct.byref(uiHandles), operationMode) 364 | 365 | handles = [] 366 | if ret == 0: 367 | for i in range(count.value): 368 | handles.append(uiHandles[i]) 369 | #free C buffers 370 | c_ReleaseBuffer(uiHandles) 371 | 372 | return ret, handles 373 | 374 | def simxLoadScene(clientID, scenePathAndName, options, operationMode): 375 | ''' 376 | Please have a look at the function description/documentation in the V-REP user manual 377 | ''' 378 | 379 | if (sys.version_info[0] == 3) and (type(scenePathAndName) is str): 380 | scenePathAndName=scenePathAndName.encode('utf-8') 381 | return c_LoadScene(clientID, scenePathAndName, options, operationMode) 382 | 383 | def simxStartSimulation(clientID, operationMode): 384 | ''' 385 | Please have a look at the function description/documentation in the V-REP user manual 386 | ''' 387 | 388 | return c_StartSimulation(clientID, operationMode) 389 | 390 | def simxPauseSimulation(clientID, operationMode): 391 | ''' 392 | Please have a look at the function description/documentation in the V-REP user manual 393 | ''' 394 | 395 | return c_PauseSimulation(clientID, operationMode) 396 | 397 | def simxStopSimulation(clientID, operationMode): 398 | ''' 399 | Please have a look at the function description/documentation in the V-REP user manual 400 | ''' 401 | 402 | return c_StopSimulation(clientID, operationMode) 403 | 404 | def simxGetUIHandle(clientID, uiName, operationMode): 405 | ''' 406 | Please have a look at the function description/documentation in the V-REP user manual 407 | ''' 408 | 409 | handle = ct.c_int() 410 | if (sys.version_info[0] == 3) and (type(uiName) is str): 411 | uiName=uiName.encode('utf-8') 412 | return c_GetUIHandle(clientID, uiName, ct.byref(handle), operationMode), handle.value 413 | 414 | def simxGetUISlider(clientID, uiHandle, uiButtonID, operationMode): 415 | ''' 416 | Please have a look at the function description/documentation in the V-REP user manual 417 | ''' 418 | 419 | position = ct.c_int() 420 | return c_GetUISlider(clientID, uiHandle, uiButtonID, ct.byref(position), operationMode), position.value 421 | 422 | def simxSetUISlider(clientID, uiHandle, uiButtonID, position, operationMode): 423 | ''' 424 | Please have a look at the function description/documentation in the V-REP user manual 425 | ''' 426 | 427 | return c_SetUISlider(clientID, uiHandle, uiButtonID, position, operationMode) 428 | 429 | def simxGetUIEventButton(clientID, uiHandle, operationMode): 430 | ''' 431 | Please have a look at the function description/documentation in the V-REP user manual 432 | ''' 433 | 434 | uiEventButtonID = ct.c_int() 435 | auxValues = (ct.c_int*2)() 436 | ret = c_GetUIEventButton(clientID, uiHandle, ct.byref(uiEventButtonID), auxValues, operationMode) 437 | arr = [] 438 | for i in range(2): 439 | arr.append(auxValues[i]) 440 | return ret, uiEventButtonID.value, arr 441 | 442 | def simxGetUIButtonProperty(clientID, uiHandle, uiButtonID, operationMode): 443 | ''' 444 | Please have a look at the function description/documentation in the V-REP user manual 445 | ''' 446 | 447 | prop = ct.c_int() 448 | return c_GetUIButtonProperty(clientID, uiHandle, uiButtonID, ct.byref(prop), operationMode), prop.value 449 | 450 | def simxSetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode): 451 | ''' 452 | Please have a look at the function description/documentation in the V-REP user manual 453 | ''' 454 | 455 | return c_SetUIButtonProperty(clientID, uiHandle, uiButtonID, prop, operationMode) 456 | 457 | def simxAddStatusbarMessage(clientID, message, operationMode): 458 | ''' 459 | Please have a look at the function description/documentation in the V-REP user manual 460 | ''' 461 | 462 | if (sys.version_info[0] == 3) and (type(message) is str): 463 | message=message.encode('utf-8') 464 | return c_AddStatusbarMessage(clientID, message, operationMode) 465 | 466 | def simxAuxiliaryConsoleOpen(clientID, title, maxLines, mode, position, size, textColor, backgroundColor, operationMode): 467 | ''' 468 | Please have a look at the function description/documentation in the V-REP user manual 469 | ''' 470 | 471 | consoleHandle = ct.c_int() 472 | if (sys.version_info[0] == 3) and (type(title) is str): 473 | title=title.encode('utf-8') 474 | if position != None: 475 | c_position = (ct.c_int*2)(*position) 476 | else: 477 | c_position = None 478 | if size != None: 479 | c_size = (ct.c_int*2)(*size) 480 | else: 481 | c_size = None 482 | if textColor != None: 483 | c_textColor = (ct.c_float*3)(*textColor) 484 | else: 485 | c_textColor = None 486 | if backgroundColor != None: 487 | c_backgroundColor = (ct.c_float*3)(*backgroundColor) 488 | else: 489 | c_backgroundColor = None 490 | return c_AuxiliaryConsoleOpen(clientID, title, maxLines, mode, c_position, c_size, c_textColor, c_backgroundColor, ct.byref(consoleHandle), operationMode), consoleHandle.value 491 | 492 | def simxAuxiliaryConsoleClose(clientID, consoleHandle, operationMode): 493 | ''' 494 | Please have a look at the function description/documentation in the V-REP user manual 495 | ''' 496 | 497 | return c_AuxiliaryConsoleClose(clientID, consoleHandle, operationMode) 498 | 499 | def simxAuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode): 500 | ''' 501 | Please have a look at the function description/documentation in the V-REP user manual 502 | ''' 503 | 504 | if (sys.version_info[0] == 3) and (type(txt) is str): 505 | txt=txt.encode('utf-8') 506 | return c_AuxiliaryConsolePrint(clientID, consoleHandle, txt, operationMode) 507 | 508 | def simxAuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode): 509 | ''' 510 | Please have a look at the function description/documentation in the V-REP user manual 511 | ''' 512 | 513 | return c_AuxiliaryConsoleShow(clientID, consoleHandle, showState, operationMode) 514 | 515 | def simxGetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, operationMode): 516 | ''' 517 | Please have a look at the function description/documentation in the V-REP user manual 518 | ''' 519 | eulerAngles = (ct.c_float*3)() 520 | ret = c_GetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode) 521 | arr = [] 522 | for i in range(3): 523 | arr.append(eulerAngles[i]) 524 | return ret, arr 525 | 526 | def simxGetObjectQuaternion(clientID, objectHandle, relativeToObjectHandle, operationMode): 527 | ''' 528 | Please have a look at the function description/documentation in the V-REP user manual 529 | ''' 530 | quaternion = (ct.c_float*4)() 531 | ret = c_GetObjectQuaternion(clientID, objectHandle, relativeToObjectHandle, quaternion, operationMode) 532 | arr = [] 533 | for i in range(4): 534 | arr.append(quaternion[i]) 535 | return ret, arr 536 | 537 | def simxGetObjectPosition(clientID, objectHandle, relativeToObjectHandle, operationMode): 538 | ''' 539 | Please have a look at the function description/documentation in the V-REP user manual 540 | ''' 541 | position = (ct.c_float*3)() 542 | ret = c_GetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode) 543 | arr = [] 544 | for i in range(3): 545 | arr.append(position[i]) 546 | return ret, arr 547 | 548 | def simxSetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, eulerAngles, operationMode): 549 | ''' 550 | Please have a look at the function description/documentation in the V-REP user manual 551 | ''' 552 | 553 | angles = (ct.c_float*3)(*eulerAngles) 554 | return c_SetObjectOrientation(clientID, objectHandle, relativeToObjectHandle, angles, operationMode) 555 | 556 | def simxSetObjectQuaternion(clientID, objectHandle, relativeToObjectHandle, quaternion, operationMode): 557 | ''' 558 | Please have a look at the function description/documentation in the V-REP user manual 559 | ''' 560 | 561 | quat = (ct.c_float*4)(*quaternion) 562 | return c_SetObjectQuaternion(clientID, objectHandle, relativeToObjectHandle, quat, operationMode) 563 | 564 | def simxSetObjectPosition(clientID, objectHandle, relativeToObjectHandle, position, operationMode): 565 | ''' 566 | Please have a look at the function description/documentation in the V-REP user manual 567 | ''' 568 | 569 | c_position = (ct.c_float*3)(*position) 570 | return c_SetObjectPosition(clientID, objectHandle, relativeToObjectHandle, c_position, operationMode) 571 | 572 | def simxSetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode): 573 | ''' 574 | Please have a look at the function description/documentation in the V-REP user manual 575 | ''' 576 | 577 | return c_SetObjectParent(clientID, objectHandle, parentObject, keepInPlace, operationMode) 578 | 579 | def simxSetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode): 580 | ''' 581 | Please have a look at the function description/documentation in the V-REP user manual 582 | ''' 583 | 584 | if sys.version_info[0] == 3: 585 | if type(upStateLabel) is str: 586 | upStateLabel=upStateLabel.encode('utf-8') 587 | if type(downStateLabel) is str: 588 | downStateLabel=downStateLabel.encode('utf-8') 589 | return c_SetUIButtonLabel(clientID, uiHandle, uiButtonID, upStateLabel, downStateLabel, operationMode) 590 | 591 | def simxGetLastErrors(clientID, operationMode): 592 | ''' 593 | Please have a look at the function description/documentation in the V-REP user manual 594 | ''' 595 | errors =[] 596 | errorCnt = ct.c_int() 597 | errorStrings = ct.POINTER(ct.c_char)() 598 | ret = c_GetLastErrors(clientID, ct.byref(errorCnt), ct.byref(errorStrings), operationMode) 599 | if ret == 0: 600 | s = 0 601 | for i in range(errorCnt.value): 602 | a = bytearray() 603 | while errorStrings[s] != b'\0': 604 | if sys.version_info[0] == 3: 605 | a.append(int.from_bytes(errorStrings[s],'big')) 606 | else: 607 | a.append(errorStrings[s]) 608 | s += 1 609 | s += 1 #skip null 610 | if sys.version_info[0] == 3: 611 | errors.append(str(a,'utf-8')) 612 | else: 613 | errors.append(str(a)) 614 | 615 | return ret, errors 616 | 617 | def simxGetArrayParameter(clientID, paramIdentifier, operationMode): 618 | ''' 619 | Please have a look at the function description/documentation in the V-REP user manual 620 | ''' 621 | paramValues = (ct.c_float*3)() 622 | ret = c_GetArrayParameter(clientID, paramIdentifier, paramValues, operationMode) 623 | arr = [] 624 | for i in range(3): 625 | arr.append(paramValues[i]) 626 | return ret, arr 627 | 628 | def simxSetArrayParameter(clientID, paramIdentifier, paramValues, operationMode): 629 | ''' 630 | Please have a look at the function description/documentation in the V-REP user manual 631 | ''' 632 | 633 | c_paramValues = (ct.c_float*3)(*paramValues) 634 | return c_SetArrayParameter(clientID, paramIdentifier, c_paramValues, operationMode) 635 | 636 | def simxGetBooleanParameter(clientID, paramIdentifier, operationMode): 637 | ''' 638 | Please have a look at the function description/documentation in the V-REP user manual 639 | ''' 640 | 641 | paramValue = ct.c_ubyte() 642 | return c_GetBooleanParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), bool(paramValue.value!=0) 643 | 644 | def simxSetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode): 645 | ''' 646 | Please have a look at the function description/documentation in the V-REP user manual 647 | ''' 648 | 649 | return c_SetBooleanParameter(clientID, paramIdentifier, paramValue, operationMode) 650 | 651 | def simxGetIntegerParameter(clientID, paramIdentifier, operationMode): 652 | ''' 653 | Please have a look at the function description/documentation in the V-REP user manual 654 | ''' 655 | 656 | paramValue = ct.c_int() 657 | return c_GetIntegerParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), paramValue.value 658 | 659 | def simxSetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode): 660 | ''' 661 | Please have a look at the function description/documentation in the V-REP user manual 662 | ''' 663 | 664 | return c_SetIntegerParameter(clientID, paramIdentifier, paramValue, operationMode) 665 | 666 | def simxGetFloatingParameter(clientID, paramIdentifier, operationMode): 667 | ''' 668 | Please have a look at the function description/documentation in the V-REP user manual 669 | ''' 670 | 671 | paramValue = ct.c_float() 672 | return c_GetFloatingParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode), paramValue.value 673 | 674 | def simxSetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode): 675 | ''' 676 | Please have a look at the function description/documentation in the V-REP user manual 677 | ''' 678 | 679 | return c_SetFloatingParameter(clientID, paramIdentifier, paramValue, operationMode) 680 | 681 | def simxGetStringParameter(clientID, paramIdentifier, operationMode): 682 | ''' 683 | Please have a look at the function description/documentation in the V-REP user manual 684 | ''' 685 | paramValue = ct.POINTER(ct.c_char)() 686 | ret = c_GetStringParameter(clientID, paramIdentifier, ct.byref(paramValue), operationMode) 687 | 688 | a = bytearray() 689 | if ret == 0: 690 | i = 0 691 | while paramValue[i] != b'\0': 692 | if sys.version_info[0] == 3: 693 | a.append(int.from_bytes(paramValue[i],'big')) 694 | else: 695 | a.append(paramValue[i]) 696 | i=i+1 697 | if sys.version_info[0] == 3: 698 | a=str(a,'utf-8') 699 | else: 700 | a=str(a) 701 | return ret, a 702 | 703 | def simxGetCollisionHandle(clientID, collisionObjectName, operationMode): 704 | ''' 705 | Please have a look at the function description/documentation in the V-REP user manual 706 | ''' 707 | 708 | handle = ct.c_int() 709 | if (sys.version_info[0] == 3) and (type(collisionObjectName) is str): 710 | collisionObjectName=collisionObjectName.encode('utf-8') 711 | return c_GetCollisionHandle(clientID, collisionObjectName, ct.byref(handle), operationMode), handle.value 712 | 713 | def simxGetCollectionHandle(clientID, collectionName, operationMode): 714 | ''' 715 | Please have a look at the function description/documentation in the V-REP user manual 716 | ''' 717 | 718 | handle = ct.c_int() 719 | if (sys.version_info[0] == 3) and (type(collectionName) is str): 720 | collectionName=collectionName.encode('utf-8') 721 | return c_GetCollectionHandle(clientID, collectionName, ct.byref(handle), operationMode), handle.value 722 | 723 | def simxGetDistanceHandle(clientID, distanceObjectName, operationMode): 724 | ''' 725 | Please have a look at the function description/documentation in the V-REP user manual 726 | ''' 727 | 728 | handle = ct.c_int() 729 | if (sys.version_info[0] == 3) and (type(distanceObjectName) is str): 730 | distanceObjectName=distanceObjectName.encode('utf-8') 731 | return c_GetDistanceHandle(clientID, distanceObjectName, ct.byref(handle), operationMode), handle.value 732 | 733 | def simxReadCollision(clientID, collisionObjectHandle, operationMode): 734 | ''' 735 | Please have a look at the function description/documentation in the V-REP user manual 736 | ''' 737 | collisionState = ct.c_ubyte() 738 | return c_ReadCollision(clientID, collisionObjectHandle, ct.byref(collisionState), operationMode), bool(collisionState.value!=0) 739 | 740 | def simxReadDistance(clientID, distanceObjectHandle, operationMode): 741 | ''' 742 | Please have a look at the function description/documentation in the V-REP user manual 743 | ''' 744 | 745 | minimumDistance = ct.c_float() 746 | return c_ReadDistance(clientID, distanceObjectHandle, ct.byref(minimumDistance), operationMode), minimumDistance.value 747 | 748 | def simxRemoveObject(clientID, objectHandle, operationMode): 749 | ''' 750 | Please have a look at the function description/documentation in the V-REP user manual 751 | ''' 752 | 753 | return c_RemoveObject(clientID, objectHandle, operationMode) 754 | 755 | def simxRemoveModel(clientID, objectHandle, operationMode): 756 | ''' 757 | Please have a look at the function description/documentation in the V-REP user manual 758 | ''' 759 | 760 | return c_RemoveModel(clientID, objectHandle, operationMode) 761 | 762 | def simxRemoveUI(clientID, uiHandle, operationMode): 763 | ''' 764 | Please have a look at the function description/documentation in the V-REP user manual 765 | ''' 766 | 767 | return c_RemoveUI(clientID, uiHandle, operationMode) 768 | 769 | def simxCloseScene(clientID, operationMode): 770 | ''' 771 | Please have a look at the function description/documentation in the V-REP user manual 772 | ''' 773 | 774 | return c_CloseScene(clientID, operationMode) 775 | 776 | def simxGetObjects(clientID, objectType, operationMode): 777 | ''' 778 | Please have a look at the function description/documentation in the V-REP user manual 779 | ''' 780 | 781 | objectCount = ct.c_int() 782 | objectHandles = ct.POINTER(ct.c_int)() 783 | 784 | ret = c_GetObjects(clientID, objectType, ct.byref(objectCount), ct.byref(objectHandles), operationMode) 785 | handles = [] 786 | if ret == 0: 787 | for i in range(objectCount.value): 788 | handles.append(objectHandles[i]) 789 | 790 | return ret, handles 791 | 792 | 793 | def simxDisplayDialog(clientID, titleText, mainText, dialogType, initialText, titleColors, dialogColors, operationMode): 794 | ''' 795 | Please have a look at the function description/documentation in the V-REP user manual 796 | ''' 797 | if titleColors != None: 798 | c_titleColors = (ct.c_float*6)(*titleColors) 799 | else: 800 | c_titleColors = None 801 | if dialogColors != None: 802 | c_dialogColors = (ct.c_float*6)(*dialogColors) 803 | else: 804 | c_dialogColors = None 805 | 806 | c_dialogHandle = ct.c_int() 807 | c_uiHandle = ct.c_int() 808 | if sys.version_info[0] == 3: 809 | if type(titleText) is str: 810 | titleText=titleText.encode('utf-8') 811 | if type(mainText) is str: 812 | mainText=mainText.encode('utf-8') 813 | if type(initialText) is str: 814 | initialText=initialText.encode('utf-8') 815 | return c_DisplayDialog(clientID, titleText, mainText, dialogType, initialText, c_titleColors, c_dialogColors, ct.byref(c_dialogHandle), ct.byref(c_uiHandle), operationMode), c_dialogHandle.value, c_uiHandle.value 816 | 817 | def simxEndDialog(clientID, dialogHandle, operationMode): 818 | ''' 819 | Please have a look at the function description/documentation in the V-REP user manual 820 | ''' 821 | 822 | return c_EndDialog(clientID, dialogHandle, operationMode) 823 | 824 | def simxGetDialogInput(clientID, dialogHandle, operationMode): 825 | ''' 826 | Please have a look at the function description/documentation in the V-REP user manual 827 | ''' 828 | inputText = ct.POINTER(ct.c_char)() 829 | ret = c_GetDialogInput(clientID, dialogHandle, ct.byref(inputText), operationMode) 830 | 831 | a = bytearray() 832 | if ret == 0: 833 | i = 0 834 | while inputText[i] != b'\0': 835 | if sys.version_info[0] == 3: 836 | a.append(int.from_bytes(inputText[i],'big')) 837 | else: 838 | a.append(inputText[i]) 839 | i = i+1 840 | 841 | if sys.version_info[0] == 3: 842 | a=str(a,'utf-8') 843 | else: 844 | a=str(a) 845 | return ret, a 846 | 847 | 848 | def simxGetDialogResult(clientID, dialogHandle, operationMode): 849 | ''' 850 | Please have a look at the function description/documentation in the V-REP user manual 851 | ''' 852 | result = ct.c_int() 853 | return c_GetDialogResult(clientID, dialogHandle, ct.byref(result), operationMode), result.value 854 | 855 | def simxCopyPasteObjects(clientID, objectHandles, operationMode): 856 | ''' 857 | Please have a look at the function description/documentation in the V-REP user manual 858 | ''' 859 | c_objectHandles = (ct.c_int*len(objectHandles))(*objectHandles) 860 | c_objectHandles = ct.cast(c_objectHandles,ct.POINTER(ct.c_int)) # IronPython needs this 861 | newObjectCount = ct.c_int() 862 | newObjectHandles = ct.POINTER(ct.c_int)() 863 | ret = c_CopyPasteObjects(clientID, c_objectHandles, len(objectHandles), ct.byref(newObjectHandles), ct.byref(newObjectCount), operationMode) 864 | 865 | newobj = [] 866 | if ret == 0: 867 | for i in range(newObjectCount.value): 868 | newobj.append(newObjectHandles[i]) 869 | 870 | return ret, newobj 871 | 872 | 873 | def simxGetObjectSelection(clientID, operationMode): 874 | ''' 875 | Please have a look at the function description/documentation in the V-REP user manual 876 | ''' 877 | objectCount = ct.c_int() 878 | objectHandles = ct.POINTER(ct.c_int)() 879 | ret = c_GetObjectSelection(clientID, ct.byref(objectHandles), ct.byref(objectCount), operationMode) 880 | 881 | newobj = [] 882 | if ret == 0: 883 | for i in range(objectCount.value): 884 | newobj.append(objectHandles[i]) 885 | 886 | return ret, newobj 887 | 888 | 889 | 890 | def simxSetObjectSelection(clientID, objectHandles, operationMode): 891 | ''' 892 | Please have a look at the function description/documentation in the V-REP user manual 893 | ''' 894 | 895 | c_objectHandles = (ct.c_int*len(objectHandles))(*objectHandles) 896 | return c_SetObjectSelection(clientID, c_objectHandles, len(objectHandles), operationMode) 897 | 898 | def simxClearFloatSignal(clientID, signalName, operationMode): 899 | ''' 900 | Please have a look at the function description/documentation in the V-REP user manual 901 | ''' 902 | 903 | if (sys.version_info[0] == 3) and (type(signalName) is str): 904 | signalName=signalName.encode('utf-8') 905 | return c_ClearFloatSignal(clientID, signalName, operationMode) 906 | 907 | def simxClearIntegerSignal(clientID, signalName, operationMode): 908 | ''' 909 | Please have a look at the function description/documentation in the V-REP user manual 910 | ''' 911 | 912 | if (sys.version_info[0] == 3) and (type(signalName) is str): 913 | signalName=signalName.encode('utf-8') 914 | return c_ClearIntegerSignal(clientID, signalName, operationMode) 915 | 916 | def simxClearStringSignal(clientID, signalName, operationMode): 917 | ''' 918 | Please have a look at the function description/documentation in the V-REP user manual 919 | ''' 920 | 921 | if (sys.version_info[0] == 3) and (type(signalName) is str): 922 | signalName=signalName.encode('utf-8') 923 | return c_ClearStringSignal(clientID, signalName, operationMode) 924 | 925 | def simxGetFloatSignal(clientID, signalName, operationMode): 926 | ''' 927 | Please have a look at the function description/documentation in the V-REP user manual 928 | ''' 929 | 930 | signalValue = ct.c_float() 931 | if (sys.version_info[0] == 3) and (type(signalName) is str): 932 | signalName=signalName.encode('utf-8') 933 | return c_GetFloatSignal(clientID, signalName, ct.byref(signalValue), operationMode), signalValue.value 934 | 935 | def simxGetIntegerSignal(clientID, signalName, operationMode): 936 | ''' 937 | Please have a look at the function description/documentation in the V-REP user manual 938 | ''' 939 | 940 | signalValue = ct.c_int() 941 | if (sys.version_info[0] == 3) and (type(signalName) is str): 942 | signalName=signalName.encode('utf-8') 943 | return c_GetIntegerSignal(clientID, signalName, ct.byref(signalValue), operationMode), signalValue.value 944 | 945 | def simxGetStringSignal(clientID, signalName, operationMode): 946 | ''' 947 | Please have a look at the function description/documentation in the V-REP user manual 948 | ''' 949 | 950 | signalLength = ct.c_int(); 951 | signalValue = ct.POINTER(ct.c_ubyte)() 952 | if (sys.version_info[0] == 3) and (type(signalName) is str): 953 | signalName=signalName.encode('utf-8') 954 | ret = c_GetStringSignal(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) 955 | 956 | a = bytearray() 957 | if ret == 0: 958 | for i in range(signalLength.value): 959 | a.append(signalValue[i]) 960 | if sys.version_info[0] != 3: 961 | a=str(a) 962 | 963 | return ret, a 964 | 965 | def simxGetAndClearStringSignal(clientID, signalName, operationMode): 966 | ''' 967 | Please have a look at the function description/documentation in the V-REP user manual 968 | ''' 969 | 970 | signalLength = ct.c_int(); 971 | signalValue = ct.POINTER(ct.c_ubyte)() 972 | if (sys.version_info[0] == 3) and (type(signalName) is str): 973 | signalName=signalName.encode('utf-8') 974 | ret = c_GetAndClearStringSignal(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) 975 | 976 | a = bytearray() 977 | if ret == 0: 978 | for i in range(signalLength.value): 979 | a.append(signalValue[i]) 980 | if sys.version_info[0] != 3: 981 | a=str(a) 982 | 983 | return ret, a 984 | 985 | def simxReadStringStream(clientID, signalName, operationMode): 986 | ''' 987 | Please have a look at the function description/documentation in the V-REP user manual 988 | ''' 989 | 990 | signalLength = ct.c_int(); 991 | signalValue = ct.POINTER(ct.c_ubyte)() 992 | if (sys.version_info[0] == 3) and (type(signalName) is str): 993 | signalName=signalName.encode('utf-8') 994 | ret = c_ReadStringStream(clientID, signalName, ct.byref(signalValue), ct.byref(signalLength), operationMode) 995 | 996 | a = bytearray() 997 | if ret == 0: 998 | for i in range(signalLength.value): 999 | a.append(signalValue[i]) 1000 | if sys.version_info[0] != 3: 1001 | a=str(a) 1002 | 1003 | return ret, a 1004 | 1005 | def simxSetFloatSignal(clientID, signalName, signalValue, operationMode): 1006 | ''' 1007 | Please have a look at the function description/documentation in the V-REP user manual 1008 | ''' 1009 | 1010 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1011 | signalName=signalName.encode('utf-8') 1012 | return c_SetFloatSignal(clientID, signalName, signalValue, operationMode) 1013 | 1014 | def simxSetIntegerSignal(clientID, signalName, signalValue, operationMode): 1015 | ''' 1016 | Please have a look at the function description/documentation in the V-REP user manual 1017 | ''' 1018 | 1019 | if (sys.version_info[0] == 3) and (type(signalName) is str): 1020 | signalName=signalName.encode('utf-8') 1021 | return c_SetIntegerSignal(clientID, signalName, signalValue, operationMode) 1022 | 1023 | def simxSetStringSignal(clientID, signalName, signalValue, operationMode): 1024 | ''' 1025 | Please have a look at the function description/documentation in the V-REP user manual 1026 | ''' 1027 | 1028 | sigV=signalValue 1029 | if sys.version_info[0] == 3: 1030 | if type(signalName) is str: 1031 | signalName=signalName.encode('utf-8') 1032 | if type(signalValue) is bytearray: 1033 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1034 | if type(signalValue) is str: 1035 | signalValue=signalValue.encode('utf-8') 1036 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1037 | else: 1038 | if type(signalValue) is bytearray: 1039 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1040 | if type(signalValue) is str: 1041 | signalValue=bytearray(signalValue) 1042 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1043 | sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1044 | return c_SetStringSignal(clientID, signalName, sigV, len(signalValue), operationMode) 1045 | 1046 | def simxAppendStringSignal(clientID, signalName, signalValue, operationMode): 1047 | ''' 1048 | Please have a look at the function description/documentation in the V-REP user manual 1049 | ''' 1050 | 1051 | sigV=signalValue 1052 | if sys.version_info[0] == 3: 1053 | if type(signalName) is str: 1054 | signalName=signalName.encode('utf-8') 1055 | if type(signalValue) is bytearray: 1056 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1057 | if type(signalValue) is str: 1058 | signalValue=signalValue.encode('utf-8') 1059 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1060 | else: 1061 | if type(signalValue) is bytearray: 1062 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1063 | if type(signalValue) is str: 1064 | signalValue=bytearray(signalValue) 1065 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1066 | sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1067 | return c_AppendStringSignal(clientID, signalName, sigV, len(signalValue), operationMode) 1068 | 1069 | def simxWriteStringStream(clientID, signalName, signalValue, operationMode): 1070 | ''' 1071 | Please have a look at the function description/documentation in the V-REP user manual 1072 | ''' 1073 | 1074 | sigV=signalValue 1075 | if sys.version_info[0] == 3: 1076 | if type(signalName) is str: 1077 | signalName=signalName.encode('utf-8') 1078 | if type(signalValue) is bytearray: 1079 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1080 | if type(signalValue) is str: 1081 | signalValue=signalValue.encode('utf-8') 1082 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1083 | else: 1084 | if type(signalValue) is bytearray: 1085 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1086 | if type(signalValue) is str: 1087 | signalValue=bytearray(signalValue) 1088 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1089 | sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1090 | return c_WriteStringStream(clientID, signalName, sigV, len(signalValue), operationMode) 1091 | 1092 | def simxGetObjectFloatParameter(clientID, objectHandle, parameterID, operationMode): 1093 | ''' 1094 | Please have a look at the function description/documentation in the V-REP user manual 1095 | ''' 1096 | 1097 | parameterValue = ct.c_float() 1098 | return c_GetObjectFloatParameter(clientID, objectHandle, parameterID, ct.byref(parameterValue), operationMode), parameterValue.value 1099 | 1100 | def simxSetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): 1101 | ''' 1102 | Please have a look at the function description/documentation in the V-REP user manual 1103 | ''' 1104 | 1105 | return c_SetObjectFloatParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) 1106 | 1107 | def simxGetObjectIntParameter(clientID, objectHandle, parameterID, operationMode): 1108 | ''' 1109 | Please have a look at the function description/documentation in the V-REP user manual 1110 | ''' 1111 | 1112 | parameterValue = ct.c_int() 1113 | return c_GetObjectIntParameter(clientID, objectHandle, parameterID, ct.byref(parameterValue), operationMode), parameterValue.value 1114 | 1115 | def simxSetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode): 1116 | ''' 1117 | Please have a look at the function description/documentation in the V-REP user manual 1118 | ''' 1119 | 1120 | return c_SetObjectIntParameter(clientID, objectHandle, parameterID, parameterValue, operationMode) 1121 | 1122 | def simxGetModelProperty(clientID, objectHandle, operationMode): 1123 | ''' 1124 | Please have a look at the function description/documentation in the V-REP user manual 1125 | ''' 1126 | prop = ct.c_int() 1127 | return c_GetModelProperty(clientID, objectHandle, ct.byref(prop), operationMode), prop.value 1128 | 1129 | def simxSetModelProperty(clientID, objectHandle, prop, operationMode): 1130 | ''' 1131 | Please have a look at the function description/documentation in the V-REP user manual 1132 | ''' 1133 | 1134 | return c_SetModelProperty(clientID, objectHandle, prop, operationMode) 1135 | 1136 | def simxStart(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs): 1137 | ''' 1138 | Please have a look at the function description/documentation in the V-REP user manual 1139 | ''' 1140 | 1141 | if (sys.version_info[0] == 3) and (type(connectionAddress) is str): 1142 | connectionAddress=connectionAddress.encode('utf-8') 1143 | return c_Start(connectionAddress, connectionPort, waitUntilConnected, doNotReconnectOnceDisconnected, timeOutInMs, commThreadCycleInMs) 1144 | 1145 | def simxFinish(clientID): 1146 | ''' 1147 | Please have a look at the function description/documentation in the V-REP user manual 1148 | ''' 1149 | 1150 | return c_Finish(clientID) 1151 | 1152 | def simxGetPingTime(clientID): 1153 | ''' 1154 | Please have a look at the function description/documentation in the V-REP user manual 1155 | ''' 1156 | pingTime = ct.c_int() 1157 | return c_GetPingTime(clientID, ct.byref(pingTime)), pingTime.value 1158 | 1159 | def simxGetLastCmdTime(clientID): 1160 | ''' 1161 | Please have a look at the function description/documentation in the V-REP user manual 1162 | ''' 1163 | 1164 | return c_GetLastCmdTime(clientID) 1165 | 1166 | def simxSynchronousTrigger(clientID): 1167 | ''' 1168 | Please have a look at the function description/documentation in the V-REP user manual 1169 | ''' 1170 | 1171 | return c_SynchronousTrigger(clientID) 1172 | 1173 | def simxSynchronous(clientID, enable): 1174 | ''' 1175 | Please have a look at the function description/documentation in the V-REP user manual 1176 | ''' 1177 | 1178 | return c_Synchronous(clientID, enable) 1179 | 1180 | def simxPauseCommunication(clientID, enable): 1181 | ''' 1182 | Please have a look at the function description/documentation in the V-REP user manual 1183 | ''' 1184 | 1185 | return c_PauseCommunication(clientID, enable) 1186 | 1187 | def simxGetInMessageInfo(clientID, infoType): 1188 | ''' 1189 | Please have a look at the function description/documentation in the V-REP user manual 1190 | ''' 1191 | info = ct.c_int() 1192 | return c_GetInMessageInfo(clientID, infoType, ct.byref(info)), info.value 1193 | 1194 | def simxGetOutMessageInfo(clientID, infoType): 1195 | ''' 1196 | Please have a look at the function description/documentation in the V-REP user manual 1197 | ''' 1198 | info = ct.c_int() 1199 | return c_GetOutMessageInfo(clientID, infoType, ct.byref(info)), info.value 1200 | 1201 | def simxGetConnectionId(clientID): 1202 | ''' 1203 | Please have a look at the function description/documentation in the V-REP user manual 1204 | ''' 1205 | 1206 | return c_GetConnectionId(clientID) 1207 | 1208 | def simxCreateBuffer(bufferSize): 1209 | ''' 1210 | Please have a look at the function description/documentation in the V-REP user manual 1211 | ''' 1212 | 1213 | return c_CreateBuffer(bufferSize) 1214 | 1215 | def simxReleaseBuffer(buffer): 1216 | ''' 1217 | Please have a look at the function description/documentation in the V-REP user manual 1218 | ''' 1219 | 1220 | return c_ReleaseBuffer(buffer) 1221 | 1222 | def simxTransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode): 1223 | ''' 1224 | Please have a look at the function description/documentation in the V-REP user manual 1225 | ''' 1226 | 1227 | if (sys.version_info[0] == 3) and (type(filePathAndName) is str): 1228 | filePathAndName=filePathAndName.encode('utf-8') 1229 | return c_TransferFile(clientID, filePathAndName, fileName_serverSide, timeOut, operationMode) 1230 | 1231 | def simxEraseFile(clientID, fileName_serverSide, operationMode): 1232 | ''' 1233 | Please have a look at the function description/documentation in the V-REP user manual 1234 | ''' 1235 | 1236 | if (sys.version_info[0] == 3) and (type(fileName_serverSide) is str): 1237 | fileName_serverSide=fileName_serverSide.encode('utf-8') 1238 | return c_EraseFile(clientID, fileName_serverSide, operationMode) 1239 | 1240 | def simxCreateDummy(clientID, size, color, operationMode): 1241 | ''' 1242 | Please have a look at the function description/documentation in the V-REP user manual 1243 | ''' 1244 | 1245 | handle = ct.c_int() 1246 | if color != None: 1247 | c_color = (ct.c_ubyte*12)(*color) 1248 | else: 1249 | c_color = None 1250 | return c_CreateDummy(clientID, size, c_color, ct.byref(handle), operationMode), handle.value 1251 | 1252 | def simxQuery(clientID, signalName, signalValue, retSignalName, timeOutInMs): 1253 | ''' 1254 | Please have a look at the function description/documentation in the V-REP user manual 1255 | ''' 1256 | 1257 | retSignalLength = ct.c_int(); 1258 | retSignalValue = ct.POINTER(ct.c_ubyte)() 1259 | 1260 | sigV=signalValue 1261 | if sys.version_info[0] == 3: 1262 | if type(signalName) is str: 1263 | signalName=signalName.encode('utf-8') 1264 | if type(retSignalName) is str: 1265 | retSignalName=retSignalName.encode('utf-8') 1266 | if type(signalValue) is bytearray: 1267 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1268 | if type(signalValue) is str: 1269 | signalValue=signalValue.encode('utf-8') 1270 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1271 | else: 1272 | if type(signalValue) is bytearray: 1273 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1274 | if type(signalValue) is str: 1275 | signalValue=bytearray(signalValue) 1276 | sigV = (ct.c_ubyte*len(signalValue))(*signalValue) 1277 | sigV=ct.cast(sigV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1278 | 1279 | ret = c_Query(clientID, signalName, sigV, len(signalValue), retSignalName, ct.byref(retSignalValue), ct.byref(retSignalLength), timeOutInMs) 1280 | 1281 | a = bytearray() 1282 | if ret == 0: 1283 | for i in range(retSignalLength.value): 1284 | a.append(retSignalValue[i]) 1285 | if sys.version_info[0] != 3: 1286 | a=str(a) 1287 | 1288 | return ret, a 1289 | 1290 | def simxGetObjectGroupData(clientID, objectType, dataType, operationMode): 1291 | ''' 1292 | Please have a look at the function description/documentation in the V-REP user manual 1293 | ''' 1294 | 1295 | handles =[] 1296 | intData =[] 1297 | floatData =[] 1298 | stringData =[] 1299 | handlesC = ct.c_int() 1300 | handlesP = ct.POINTER(ct.c_int)() 1301 | intDataC = ct.c_int() 1302 | intDataP = ct.POINTER(ct.c_int)() 1303 | floatDataC = ct.c_int() 1304 | floatDataP = ct.POINTER(ct.c_float)() 1305 | stringDataC = ct.c_int() 1306 | stringDataP = ct.POINTER(ct.c_char)() 1307 | ret = c_GetObjectGroupData(clientID, objectType, dataType, ct.byref(handlesC), ct.byref(handlesP), ct.byref(intDataC), ct.byref(intDataP), ct.byref(floatDataC), ct.byref(floatDataP), ct.byref(stringDataC), ct.byref(stringDataP), operationMode) 1308 | 1309 | if ret == 0: 1310 | for i in range(handlesC.value): 1311 | handles.append(handlesP[i]) 1312 | for i in range(intDataC.value): 1313 | intData.append(intDataP[i]) 1314 | for i in range(floatDataC.value): 1315 | floatData.append(floatDataP[i]) 1316 | s = 0 1317 | for i in range(stringDataC.value): 1318 | a = bytearray() 1319 | while stringDataP[s] != b'\0': 1320 | if sys.version_info[0] == 3: 1321 | a.append(int.from_bytes(stringDataP[s],'big')) 1322 | else: 1323 | a.append(stringDataP[s]) 1324 | s += 1 1325 | s += 1 #skip null 1326 | if sys.version_info[0] == 3: 1327 | a=str(a,'utf-8') 1328 | else: 1329 | a=str(a) 1330 | stringData.append(a) 1331 | 1332 | return ret, handles, intData, floatData, stringData 1333 | 1334 | def simxCallScriptFunction(clientID, scriptDescription, options, functionName, inputInts, inputFloats, inputStrings, inputBuffer, operationMode): 1335 | ''' 1336 | Please have a look at the function description/documentation in the V-REP user manual 1337 | ''' 1338 | 1339 | inputBufferV=inputBuffer 1340 | if sys.version_info[0] == 3: 1341 | if type(scriptDescription) is str: 1342 | scriptDescription=scriptDescription.encode('utf-8') 1343 | if type(functionName) is str: 1344 | functionName=functionName.encode('utf-8') 1345 | if type(inputBuffer) is bytearray: 1346 | inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) 1347 | if type(inputBuffer) is str: 1348 | inputBuffer=inputBuffer.encode('utf-8') 1349 | inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) 1350 | else: 1351 | if type(inputBuffer) is bytearray: 1352 | inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) 1353 | if type(inputBuffer) is str: 1354 | inputBuffer=bytearray(inputBuffer) 1355 | inputBufferV = (ct.c_ubyte*len(inputBuffer))(*inputBuffer) 1356 | inputBufferV=ct.cast(inputBufferV,ct.POINTER(ct.c_ubyte)) # IronPython needs this 1357 | 1358 | c_inInts = (ct.c_int*len(inputInts))(*inputInts) 1359 | c_inInts = ct.cast(c_inInts,ct.POINTER(ct.c_int)) # IronPython needs this 1360 | c_inFloats = (ct.c_float*len(inputFloats))(*inputFloats) 1361 | c_inFloats = ct.cast(c_inFloats,ct.POINTER(ct.c_float)) # IronPython needs this 1362 | 1363 | concatStr=''.encode('utf-8') 1364 | for i in range(len(inputStrings)): 1365 | a=inputStrings[i] 1366 | a=a+'\0' 1367 | if type(a) is str: 1368 | a=a.encode('utf-8') 1369 | concatStr=concatStr+a 1370 | c_inStrings = (ct.c_char*len(concatStr))(*concatStr) 1371 | 1372 | intDataOut =[] 1373 | floatDataOut =[] 1374 | stringDataOut =[] 1375 | bufferOut =bytearray() 1376 | 1377 | intDataC = ct.c_int() 1378 | intDataP = ct.POINTER(ct.c_int)() 1379 | floatDataC = ct.c_int() 1380 | floatDataP = ct.POINTER(ct.c_float)() 1381 | stringDataC = ct.c_int() 1382 | stringDataP = ct.POINTER(ct.c_char)() 1383 | bufferS = ct.c_int() 1384 | bufferP = ct.POINTER(ct.c_ubyte)() 1385 | 1386 | ret = c_CallScriptFunction(clientID,scriptDescription,options,functionName,len(inputInts),c_inInts,len(inputFloats),c_inFloats,len(inputStrings),c_inStrings,len(inputBuffer),inputBufferV,ct.byref(intDataC),ct.byref(intDataP),ct.byref(floatDataC),ct.byref(floatDataP),ct.byref(stringDataC),ct.byref(stringDataP),ct.byref(bufferS),ct.byref(bufferP),operationMode) 1387 | 1388 | if ret == 0: 1389 | for i in range(intDataC.value): 1390 | intDataOut.append(intDataP[i]) 1391 | for i in range(floatDataC.value): 1392 | floatDataOut.append(floatDataP[i]) 1393 | s = 0 1394 | for i in range(stringDataC.value): 1395 | a = bytearray() 1396 | while stringDataP[s] != b'\0': 1397 | if sys.version_info[0] == 3: 1398 | a.append(int.from_bytes(stringDataP[s],'big')) 1399 | else: 1400 | a.append(stringDataP[s]) 1401 | s += 1 1402 | s += 1 #skip null 1403 | if sys.version_info[0] == 3: 1404 | a=str(a,'utf-8') 1405 | else: 1406 | a=str(a) 1407 | stringDataOut.append(a) 1408 | for i in range(bufferS.value): 1409 | bufferOut.append(bufferP[i]) 1410 | if sys.version_info[0] != 3: 1411 | bufferOut=str(bufferOut) 1412 | 1413 | return ret, intDataOut, floatDataOut, stringDataOut, bufferOut 1414 | 1415 | def simxGetObjectVelocity(clientID, objectHandle, operationMode): 1416 | ''' 1417 | Please have a look at the function description/documentation in the V-REP user manual 1418 | ''' 1419 | linearVel = (ct.c_float*3)() 1420 | angularVel = (ct.c_float*3)() 1421 | ret = c_GetObjectVelocity(clientID, objectHandle, linearVel, angularVel, operationMode) 1422 | arr1 = [] 1423 | for i in range(3): 1424 | arr1.append(linearVel[i]) 1425 | arr2 = [] 1426 | for i in range(3): 1427 | arr2.append(angularVel[i]) 1428 | return ret, arr1, arr2 1429 | 1430 | def simxPackInts(intList): 1431 | ''' 1432 | Please have a look at the function description/documentation in the V-REP user manual 1433 | ''' 1434 | 1435 | if sys.version_info[0] == 3: 1436 | s=bytes() 1437 | for i in range(len(intList)): 1438 | s=s+struct.pack('=1x) 234 | # reserved =sim_simulation_advancing|0x02 235 | sim_simulation_advancing_lastbeforepause =sim_simulation_advancing|0x03 # Last simulation pass before pause (1x) 236 | sim_simulation_advancing_firstafterpause =sim_simulation_advancing|0x04 # First simulation pass after pause (1x) 237 | sim_simulation_advancing_abouttostop =sim_simulation_advancing|0x05 # "Trying to stop" simulation pass (>=1x) 238 | sim_simulation_advancing_lastbeforestop =sim_simulation_advancing|0x06 # Last simulation pass (1x) 239 | 240 | 241 | # Script execution result (first return value) 242 | sim_script_no_error =0 243 | sim_script_main_script_nonexistent =1 244 | sim_script_main_script_not_called =2 245 | sim_script_reentrance_error =4 246 | sim_script_lua_error =8 247 | sim_script_call_error =16 248 | 249 | 250 | # Script types (serialized!) 251 | sim_scripttype_mainscript =0 252 | sim_scripttype_childscript =1 253 | sim_scripttype_jointctrlcallback =4 254 | sim_scripttype_contactcallback =5 255 | sim_scripttype_customizationscript =6 256 | sim_scripttype_generalcallback =7 257 | 258 | # API call error messages 259 | sim_api_errormessage_ignore =0 # does not memorize nor output errors 260 | sim_api_errormessage_report =1 # memorizes errors (default for C-API calls) 261 | sim_api_errormessage_output =2 # memorizes and outputs errors (default for Lua-API calls) 262 | 263 | 264 | # special argument of some functions 265 | sim_handle_all =-2 266 | sim_handle_all_except_explicit =-3 267 | sim_handle_self =-4 268 | sim_handle_main_script =-5 269 | sim_handle_tree =-6 270 | sim_handle_chain =-7 271 | sim_handle_single =-8 272 | sim_handle_default =-9 273 | sim_handle_all_except_self =-10 274 | sim_handle_parent =-11 275 | 276 | 277 | # special handle flags 278 | sim_handleflag_assembly =0x400000 279 | sim_handleflag_model =0x800000 280 | 281 | 282 | # distance calculation methods (serialized) 283 | sim_distcalcmethod_dl =0 284 | sim_distcalcmethod_dac =1 285 | sim_distcalcmethod_max_dl_dac =2 286 | sim_distcalcmethod_dl_and_dac =3 287 | sim_distcalcmethod_sqrt_dl2_and_dac2=4 288 | sim_distcalcmethod_dl_if_nonzero =5 289 | sim_distcalcmethod_dac_if_nonzero =6 290 | 291 | 292 | # Generic dialog styles 293 | sim_dlgstyle_message =0 294 | sim_dlgstyle_input =1 295 | sim_dlgstyle_ok =2 296 | sim_dlgstyle_ok_cancel =3 297 | sim_dlgstyle_yes_no =4 298 | sim_dlgstyle_dont_center =32# can be combined with one of above values. Only with this flag can the position of the related UI be set just after dialog creation 299 | 300 | # Generic dialog return values 301 | sim_dlgret_still_open =0 302 | sim_dlgret_ok =1 303 | sim_dlgret_cancel =2 304 | sim_dlgret_yes =3 305 | sim_dlgret_no =4 306 | 307 | 308 | # Path properties 309 | sim_pathproperty_show_line =0x0001 310 | sim_pathproperty_show_orientation =0x0002 311 | sim_pathproperty_closed_path =0x0004 312 | sim_pathproperty_automatic_orientation =0x0008 313 | sim_pathproperty_invert_velocity =0x0010 314 | sim_pathproperty_infinite_acceleration =0x0020 315 | sim_pathproperty_flat_path =0x0040 316 | sim_pathproperty_show_position =0x0080 317 | sim_pathproperty_auto_velocity_profile_translation =0x0100 318 | sim_pathproperty_auto_velocity_profile_rotation =0x0200 319 | sim_pathproperty_endpoints_at_zero =0x0400 320 | sim_pathproperty_keep_x_up =0x0800 321 | 322 | 323 | # drawing objects 324 | # following are mutually exclusive 325 | sim_drawing_points =0 # 3 values per point (point size in pixels) 326 | sim_drawing_lines =1 # 6 values per line (line size in pixels) 327 | sim_drawing_triangles =2 # 9 values per triangle 328 | sim_drawing_trianglepoints =3 # 6 values per point (3 for triangle position 3 for triangle normal vector) (triangle size in meters) 329 | sim_drawing_quadpoints =4 # 6 values per point (3 for quad position 3 for quad normal vector) (quad size in meters) 330 | sim_drawing_discpoints =5 # 6 values per point (3 for disc position 3 for disc normal vector) (disc size in meters) 331 | sim_drawing_cubepoints =6 # 6 values per point (3 for cube position 3 for cube normal vector) (cube size in meters) 332 | sim_drawing_spherepoints =7 # 3 values per point (sphere size in meters) 333 | 334 | # following can be or-combined 335 | sim_drawing_itemcolors =0x00020 # +3 values per item (each item has its own ambient color (rgb values)). 336 | # Mutually exclusive with sim_drawing_vertexcolors 337 | sim_drawing_vertexcolors =0x00040 # +3 values per vertex (each vertex has its own ambient color (rgb values). Only for sim_drawing_lines (+6) and for sim_drawing_triangles(+9)). Mutually exclusive with sim_drawing_itemcolors 338 | sim_drawing_itemsizes =0x00080 # +1 value per item (each item has its own size). Not for sim_drawing_triangles 339 | sim_drawing_backfaceculling =0x00100 # back faces are not displayed for all items 340 | sim_drawing_wireframe =0x00200 # all items displayed in wireframe 341 | sim_drawing_painttag =0x00400 # all items are tagged as paint (for additinal processing at a later stage) 342 | sim_drawing_followparentvisibility =0x00800 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible 343 | sim_drawing_cyclic =0x01000 # if the max item count was reached then the first items are overwritten. 344 | sim_drawing_50percenttransparency =0x02000 # the drawing object will be 50% transparent 345 | sim_drawing_25percenttransparency =0x04000 # the drawing object will be 25% transparent 346 | sim_drawing_12percenttransparency =0x08000 # the drawing object will be 12.5% transparent 347 | sim_drawing_emissioncolor =0x10000 # When used in combination with sim_drawing_itemcolors or sim_drawing_vertexcolors then the specified colors will be for the emissive component 348 | sim_drawing_facingcamera =0x20000 # Only for trianglepoints quadpoints discpoints and cubepoints. If specified the normal verctor is calculated to face the camera (each item data requires 3 values less) 349 | sim_drawing_overlay =0x40000 # When specified objects are always drawn on top of "regular objects" 350 | sim_drawing_itemtransparency =0x80000 # +1 value per item (each item has its own transparency value (0-1)). Not compatible with sim_drawing_vertexcolors 351 | 352 | # banner values 353 | # following can be or-combined 354 | sim_banner_left =0x00001 # Banners display on the left of the specified point 355 | sim_banner_right =0x00002 # Banners display on the right of the specified point 356 | sim_banner_nobackground =0x00004 # Banners have no background rectangle 357 | sim_banner_overlay =0x00008 # When specified banners are always drawn on top of "regular objects" 358 | sim_banner_followparentvisibility =0x00010 # if the object is associated with a scene object then it follows that visibility otherwise it is always visible 359 | sim_banner_clickselectsparent =0x00020 # if the object is associated with a scene object then clicking the banner will select the scene object 360 | sim_banner_clicktriggersevent =0x00040 # if the banner is clicked an event is triggered (sim_message_eventcallback_bannerclicked and sim_message_bannerclicked are generated) 361 | sim_banner_facingcamera =0x00080 # If specified the banner will always face the camera by rotating around the banner's vertical axis (y-axis) 362 | sim_banner_fullyfacingcamera =0x00100 # If specified the banner will always fully face the camera (the banner's orientation is same as the camera looking at it) 363 | sim_banner_backfaceculling =0x00200 # If specified the banner will only be visible from one side 364 | sim_banner_keepsamesize =0x00400 # If specified the banner will always appear in the same size. In that case size represents the character height in pixels 365 | sim_banner_bitmapfont =0x00800 # If specified a fixed-size bitmap font is used. The text will also always fully face the camera and be right 366 | # to the specified position. Bitmap fonts are not clickable 367 | 368 | 369 | # particle objects following are mutually exclusive 370 | sim_particle_points1 =0 # 6 values per point (pt1 and pt2. Pt1 is start position pt2-pt1 is the initial velocity vector). i 371 | #Point is 1 pixel big. Only appearance is a point internally handled as a perfect sphere 372 | sim_particle_points2 =1 # 6 values per point. Point is 2 pixel big. Only appearance is a point internally handled as a perfect sphere 373 | sim_particle_points4 =2 # 6 values per point. Point is 4 pixel big. Only appearance is a point internally handled as a perfect sphere 374 | sim_particle_roughspheres =3 # 6 values per sphere. Only appearance is rough. Internally a perfect sphere 375 | sim_particle_spheres =4 # 6 values per sphere. Internally a perfect sphere 376 | 377 | 378 | 379 | 380 | # following can be or-combined 381 | sim_particle_respondable1to4 =0x0020 # the particles are respondable against shapes (against all objects that have at least one bit 1-4 activated in the global respondable mask) 382 | sim_particle_respondable5to8 =0x0040 # the particles are respondable against shapes (against all objects that have at least one bit 5-8 activated in the global respondable mask) 383 | sim_particle_particlerespondable =0x0080 # the particles are respondable against each other 384 | sim_particle_ignoresgravity =0x0100 # the particles ignore the effect of gravity. Not compatible with sim_particle_water 385 | sim_particle_invisible =0x0200 # the particles are invisible 386 | sim_particle_itemsizes =0x0400 # +1 value per particle (each particle can have a different size) 387 | sim_particle_itemdensities =0x0800 # +1 value per particle (each particle can have a different density) 388 | sim_particle_itemcolors =0x1000 # +3 values per particle (each particle can have a different color) 389 | sim_particle_cyclic =0x2000 # if the max item count was reached then the first items are overwritten. 390 | sim_particle_emissioncolor =0x4000 # When used in combination with sim_particle_itemcolors then the specified colors will be for the emissive component 391 | sim_particle_water =0x8000 # the particles are water particles (no weight in the water (i.e. when z<0)). Not compatible with sim_particle_ignoresgravity 392 | sim_particle_painttag =0x10000 # The particles can be seen by vision sensors (sim_particle_invisible must not be set) 393 | 394 | 395 | 396 | 397 | # custom user interface menu attributes 398 | sim_ui_menu_title =1 399 | sim_ui_menu_minimize =2 400 | sim_ui_menu_close =4 401 | sim_ui_menu_systemblock =8 402 | 403 | 404 | 405 | # Boolean parameters 406 | sim_boolparam_hierarchy_visible =0 407 | sim_boolparam_console_visible =1 408 | sim_boolparam_collision_handling_enabled =2 409 | sim_boolparam_distance_handling_enabled =3 410 | sim_boolparam_ik_handling_enabled =4 411 | sim_boolparam_gcs_handling_enabled =5 412 | sim_boolparam_dynamics_handling_enabled =6 413 | sim_boolparam_joint_motion_handling_enabled =7 414 | sim_boolparam_path_motion_handling_enabled =8 415 | sim_boolparam_proximity_sensor_handling_enabled =9 416 | sim_boolparam_vision_sensor_handling_enabled =10 417 | sim_boolparam_mill_handling_enabled =11 418 | sim_boolparam_browser_visible =12 419 | sim_boolparam_scene_and_model_load_messages =13 420 | sim_reserved0 =14 421 | sim_boolparam_shape_textures_are_visible =15 422 | sim_boolparam_display_enabled =16 423 | sim_boolparam_infotext_visible =17 424 | sim_boolparam_statustext_open =18 425 | sim_boolparam_fog_enabled =19 426 | sim_boolparam_rml2_available =20 427 | sim_boolparam_rml4_available =21 428 | sim_boolparam_mirrors_enabled =22 429 | sim_boolparam_aux_clip_planes_enabled =23 430 | sim_boolparam_full_model_copy_from_api =24 431 | sim_boolparam_realtime_simulation =25 432 | sim_boolparam_force_show_wireless_emission =27 433 | sim_boolparam_force_show_wireless_reception =28 434 | sim_boolparam_video_recording_triggered =29 435 | sim_boolparam_threaded_rendering_enabled =32 436 | sim_boolparam_fullscreen =33 437 | sim_boolparam_headless =34 438 | sim_boolparam_hierarchy_toolbarbutton_enabled =35 439 | sim_boolparam_browser_toolbarbutton_enabled =36 440 | sim_boolparam_objectshift_toolbarbutton_enabled =37 441 | sim_boolparam_objectrotate_toolbarbutton_enabled=38 442 | sim_boolparam_force_calcstruct_all_visible =39 443 | sim_boolparam_force_calcstruct_all =40 444 | sim_boolparam_exit_request =41 445 | sim_boolparam_play_toolbarbutton_enabled =42 446 | sim_boolparam_pause_toolbarbutton_enabled =43 447 | sim_boolparam_stop_toolbarbutton_enabled =44 448 | sim_boolparam_waiting_for_trigger =45 449 | 450 | 451 | # Integer parameters 452 | sim_intparam_error_report_mode =0 # Check sim_api_errormessage_... constants above for valid values 453 | sim_intparam_program_version =1 # e.g Version 2.1.4 --> 20104. Can only be read 454 | sim_intparam_instance_count =2 # do not use anymore (always returns 1 since V-REP 2.5.11) 455 | sim_intparam_custom_cmd_start_id =3 # can only be read 456 | sim_intparam_compilation_version =4 # 0=evaluation version 1=full version 2=player version. Can only be read 457 | sim_intparam_current_page =5 458 | sim_intparam_flymode_camera_handle =6 # can only be read 459 | sim_intparam_dynamic_step_divider =7 # can only be read 460 | sim_intparam_dynamic_engine =8 # 0=Bullet 1=ODE. 2=Vortex. 461 | sim_intparam_server_port_start =9 # can only be read 462 | sim_intparam_server_port_range =10 # can only be read 463 | sim_intparam_visible_layers =11 464 | sim_intparam_infotext_style =12 465 | sim_intparam_settings =13 466 | sim_intparam_edit_mode_type =14 # can only be read 467 | sim_intparam_server_port_next =15 # is initialized at sim_intparam_server_port_start 468 | sim_intparam_qt_version =16 # version of the used Qt framework 469 | sim_intparam_event_flags_read =17 # can only be read 470 | sim_intparam_event_flags_read_clear =18 # can only be read 471 | sim_intparam_platform =19 # can only be read 472 | sim_intparam_scene_unique_id =20 # can only be read 473 | sim_intparam_work_thread_count =21 474 | sim_intparam_mouse_x =22 475 | sim_intparam_mouse_y =23 476 | sim_intparam_core_count =24 477 | sim_intparam_work_thread_calc_time_ms =25 478 | sim_intparam_idle_fps =26 479 | sim_intparam_prox_sensor_select_down =27 480 | sim_intparam_prox_sensor_select_up =28 481 | sim_intparam_stop_request_counter =29 482 | sim_intparam_program_revision =30 483 | sim_intparam_mouse_buttons =31 484 | sim_intparam_dynamic_warning_disabled_mask =32 485 | sim_intparam_simulation_warning_disabled_mask =33 486 | sim_intparam_scene_index =34 487 | sim_intparam_motionplanning_seed =35 488 | sim_intparam_speedmodifier =36 489 | 490 | # Float parameters 491 | sim_floatparam_rand=0 # random value (0.0-1.0) 492 | sim_floatparam_simulation_time_step =1 493 | sim_floatparam_stereo_distance =2 494 | 495 | # String parameters 496 | sim_stringparam_application_path=0 # path of V-REP's executable 497 | sim_stringparam_video_filename=1 498 | sim_stringparam_app_arg1 =2 499 | sim_stringparam_app_arg2 =3 500 | sim_stringparam_app_arg3 =4 501 | sim_stringparam_app_arg4 =5 502 | sim_stringparam_app_arg5 =6 503 | sim_stringparam_app_arg6 =7 504 | sim_stringparam_app_arg7 =8 505 | sim_stringparam_app_arg8 =9 506 | sim_stringparam_app_arg9 =10 507 | sim_stringparam_scene_path_and_name =13 508 | 509 | # Array parameters 510 | sim_arrayparam_gravity =0 511 | sim_arrayparam_fog =1 512 | sim_arrayparam_fog_color =2 513 | sim_arrayparam_background_color1=3 514 | sim_arrayparam_background_color2=4 515 | sim_arrayparam_ambient_light =5 516 | sim_arrayparam_random_euler =6 517 | 518 | sim_objintparam_visibility_layer= 10 519 | sim_objfloatparam_abs_x_velocity= 11 520 | sim_objfloatparam_abs_y_velocity= 12 521 | sim_objfloatparam_abs_z_velocity= 13 522 | sim_objfloatparam_abs_rot_velocity= 14 523 | sim_objfloatparam_objbbox_min_x= 15 524 | sim_objfloatparam_objbbox_min_y= 16 525 | sim_objfloatparam_objbbox_min_z= 17 526 | sim_objfloatparam_objbbox_max_x= 18 527 | sim_objfloatparam_objbbox_max_y= 19 528 | sim_objfloatparam_objbbox_max_z= 20 529 | sim_objfloatparam_modelbbox_min_x= 21 530 | sim_objfloatparam_modelbbox_min_y= 22 531 | sim_objfloatparam_modelbbox_min_z= 23 532 | sim_objfloatparam_modelbbox_max_x= 24 533 | sim_objfloatparam_modelbbox_max_y= 25 534 | sim_objfloatparam_modelbbox_max_z= 26 535 | sim_objintparam_collection_self_collision_indicator= 27 536 | sim_objfloatparam_transparency_offset= 28 537 | sim_objintparam_child_role= 29 538 | sim_objintparam_parent_role= 30 539 | sim_objintparam_manipulation_permissions= 31 540 | sim_objintparam_illumination_handle= 32 541 | 542 | sim_visionfloatparam_near_clipping= 1000 543 | sim_visionfloatparam_far_clipping= 1001 544 | sim_visionintparam_resolution_x= 1002 545 | sim_visionintparam_resolution_y= 1003 546 | sim_visionfloatparam_perspective_angle= 1004 547 | sim_visionfloatparam_ortho_size= 1005 548 | sim_visionintparam_disabled_light_components= 1006 549 | sim_visionintparam_rendering_attributes= 1007 550 | sim_visionintparam_entity_to_render= 1008 551 | sim_visionintparam_windowed_size_x= 1009 552 | sim_visionintparam_windowed_size_y= 1010 553 | sim_visionintparam_windowed_pos_x= 1011 554 | sim_visionintparam_windowed_pos_y= 1012 555 | sim_visionintparam_pov_focal_blur= 1013 556 | sim_visionfloatparam_pov_blur_distance= 1014 557 | sim_visionfloatparam_pov_aperture= 1015 558 | sim_visionintparam_pov_blur_sampled= 1016 559 | sim_visionintparam_render_mode= 1017 560 | 561 | sim_jointintparam_motor_enabled= 2000 562 | sim_jointintparam_ctrl_enabled= 2001 563 | sim_jointfloatparam_pid_p= 2002 564 | sim_jointfloatparam_pid_i= 2003 565 | sim_jointfloatparam_pid_d= 2004 566 | sim_jointfloatparam_intrinsic_x= 2005 567 | sim_jointfloatparam_intrinsic_y= 2006 568 | sim_jointfloatparam_intrinsic_z= 2007 569 | sim_jointfloatparam_intrinsic_qx= 2008 570 | sim_jointfloatparam_intrinsic_qy= 2009 571 | sim_jointfloatparam_intrinsic_qz= 2010 572 | sim_jointfloatparam_intrinsic_qw= 2011 573 | sim_jointfloatparam_velocity= 2012 574 | sim_jointfloatparam_spherical_qx= 2013 575 | sim_jointfloatparam_spherical_qy= 2014 576 | sim_jointfloatparam_spherical_qz= 2015 577 | sim_jointfloatparam_spherical_qw= 2016 578 | sim_jointfloatparam_upper_limit= 2017 579 | sim_jointfloatparam_kc_k= 2018 580 | sim_jointfloatparam_kc_c= 2019 581 | sim_jointfloatparam_ik_weight= 2021 582 | sim_jointfloatparam_error_x= 2022 583 | sim_jointfloatparam_error_y= 2023 584 | sim_jointfloatparam_error_z= 2024 585 | sim_jointfloatparam_error_a= 2025 586 | sim_jointfloatparam_error_b= 2026 587 | sim_jointfloatparam_error_g= 2027 588 | sim_jointfloatparam_error_pos= 2028 589 | sim_jointfloatparam_error_angle= 2029 590 | sim_jointintparam_velocity_lock= 2030 591 | sim_jointintparam_vortex_dep_handle= 2031 592 | sim_jointfloatparam_vortex_dep_multiplication= 2032 593 | sim_jointfloatparam_vortex_dep_offset= 2033 594 | 595 | sim_shapefloatparam_init_velocity_x= 3000 596 | sim_shapefloatparam_init_velocity_y= 3001 597 | sim_shapefloatparam_init_velocity_z= 3002 598 | sim_shapeintparam_static= 3003 599 | sim_shapeintparam_respondable= 3004 600 | sim_shapefloatparam_mass= 3005 601 | sim_shapefloatparam_texture_x= 3006 602 | sim_shapefloatparam_texture_y= 3007 603 | sim_shapefloatparam_texture_z= 3008 604 | sim_shapefloatparam_texture_a= 3009 605 | sim_shapefloatparam_texture_b= 3010 606 | sim_shapefloatparam_texture_g= 3011 607 | sim_shapefloatparam_texture_scaling_x= 3012 608 | sim_shapefloatparam_texture_scaling_y= 3013 609 | sim_shapeintparam_culling= 3014 610 | sim_shapeintparam_wireframe= 3015 611 | sim_shapeintparam_compound= 3016 612 | sim_shapeintparam_convex= 3017 613 | sim_shapeintparam_convex_check= 3018 614 | sim_shapeintparam_respondable_mask= 3019 615 | sim_shapefloatparam_init_velocity_a= 3020 616 | sim_shapefloatparam_init_velocity_b= 3021 617 | sim_shapefloatparam_init_velocity_g= 3022 618 | sim_shapestringparam_color_name= 3023 619 | sim_shapeintparam_edge_visibility= 3024 620 | sim_shapefloatparam_shading_angle= 3025 621 | sim_shapefloatparam_edge_angle= 3026 622 | sim_shapeintparam_edge_borders_hidden= 3027 623 | 624 | sim_proxintparam_ray_invisibility= 4000 625 | 626 | sim_forcefloatparam_error_x= 5000 627 | sim_forcefloatparam_error_y= 5001 628 | sim_forcefloatparam_error_z= 5002 629 | sim_forcefloatparam_error_a= 5003 630 | sim_forcefloatparam_error_b= 5004 631 | sim_forcefloatparam_error_g= 5005 632 | sim_forcefloatparam_error_pos= 5006 633 | sim_forcefloatparam_error_angle= 5007 634 | 635 | sim_lightintparam_pov_casts_shadows= 8000 636 | 637 | sim_cameraintparam_disabled_light_components= 9000 638 | sim_camerafloatparam_perspective_angle= 9001 639 | sim_camerafloatparam_ortho_size= 9002 640 | sim_cameraintparam_rendering_attributes= 9003 641 | sim_cameraintparam_pov_focal_blur= 9004 642 | sim_camerafloatparam_pov_blur_distance= 9005 643 | sim_camerafloatparam_pov_aperture= 9006 644 | sim_cameraintparam_pov_blur_samples= 9007 645 | 646 | sim_dummyintparam_link_type= 10000 647 | 648 | sim_mirrorfloatparam_width= 12000 649 | sim_mirrorfloatparam_height= 12001 650 | sim_mirrorfloatparam_reflectance= 12002 651 | sim_mirrorintparam_enable= 12003 652 | 653 | sim_pplanfloatparam_x_min= 20000 654 | sim_pplanfloatparam_x_range= 20001 655 | sim_pplanfloatparam_y_min= 20002 656 | sim_pplanfloatparam_y_range= 20003 657 | sim_pplanfloatparam_z_min= 20004 658 | sim_pplanfloatparam_z_range= 20005 659 | sim_pplanfloatparam_delta_min= 20006 660 | sim_pplanfloatparam_delta_range= 20007 661 | 662 | sim_mplanintparam_nodes_computed= 25000 663 | sim_mplanintparam_prepare_nodes= 25001 664 | sim_mplanintparam_clear_nodes= 25002 665 | 666 | # User interface elements 667 | sim_gui_menubar =0x0001 668 | sim_gui_popups =0x0002 669 | sim_gui_toolbar1 =0x0004 670 | sim_gui_toolbar2 =0x0008 671 | sim_gui_hierarchy =0x0010 672 | sim_gui_infobar =0x0020 673 | sim_gui_statusbar =0x0040 674 | sim_gui_scripteditor =0x0080 675 | sim_gui_scriptsimulationparameters =0x0100 676 | sim_gui_dialogs =0x0200 677 | sim_gui_browser =0x0400 678 | sim_gui_all =0xffff 679 | 680 | 681 | # Joint modes 682 | sim_jointmode_passive =0 683 | sim_jointmode_motion =1 684 | sim_jointmode_ik =2 685 | sim_jointmode_ikdependent =3 686 | sim_jointmode_dependent =4 687 | sim_jointmode_force =5 688 | 689 | 690 | # Navigation and selection modes with the mouse. Lower byte values are mutually exclusive upper byte bits can be combined 691 | sim_navigation_passive =0x0000 692 | sim_navigation_camerashift =0x0001 693 | sim_navigation_camerarotate =0x0002 694 | sim_navigation_camerazoom =0x0003 695 | sim_navigation_cameratilt =0x0004 696 | sim_navigation_cameraangle =0x0005 697 | sim_navigation_camerafly =0x0006 698 | sim_navigation_objectshift =0x0007 699 | sim_navigation_objectrotate =0x0008 700 | sim_navigation_reserved2 =0x0009 701 | sim_navigation_reserved3 =0x000A 702 | sim_navigation_jointpathtest =0x000B 703 | sim_navigation_ikmanip =0x000C 704 | sim_navigation_objectmultipleselection =0x000D 705 | # Bit-combine following values and add them to one of above's values for a valid navigation mode 706 | sim_navigation_reserved4 =0x0100 707 | sim_navigation_clickselection =0x0200 708 | sim_navigation_ctrlselection =0x0400 709 | sim_navigation_shiftselection =0x0800 710 | sim_navigation_camerazoomwheel =0x1000 711 | sim_navigation_camerarotaterightbutton =0x2000 712 | 713 | 714 | 715 | #Remote API constants 716 | SIMX_VERSION =0 717 | # Remote API message header structure 718 | SIMX_HEADER_SIZE =18 719 | simx_headeroffset_crc =0 # 1 simxUShort. Generated by the client or server. The CRC for the message 720 | simx_headeroffset_version =2 # 1 byte. Generated by the client or server. The version of the remote API software 721 | simx_headeroffset_message_id =3 # 1 simxInt. Generated by the client (and used in a reply by the server) 722 | simx_headeroffset_client_time =7 # 1 simxInt. Client time stamp generated by the client (and sent back by the server) 723 | simx_headeroffset_server_time =11 # 1 simxInt. Generated by the server when a reply is generated. The server timestamp 724 | simx_headeroffset_scene_id =15 # 1 simxUShort. Generated by the server. A unique ID identifying the scene currently displayed 725 | simx_headeroffset_server_state =17 # 1 byte. Generated by the server. Bit coded 0 set --> simulation not stopped 1 set --> simulation paused 2 set --> real-time switch on 3-5 edit mode type (0=no edit mode 1=triangle 2=vertex 3=edge 4=path 5=UI) 726 | 727 | # Remote API command header 728 | SIMX_SUBHEADER_SIZE =26 729 | simx_cmdheaderoffset_mem_size =0 # 1 simxInt. Generated by the client or server. The buffer size of the command. 730 | simx_cmdheaderoffset_full_mem_size =4 # 1 simxInt. Generated by the client or server. The full buffer size of the command (applies to split chunks). 731 | simx_cmdheaderoffset_pdata_offset0 =8 # 1 simxUShort. Generated by the client or server. The amount of data that is part of the command identification. 732 | simx_cmdheaderoffset_pdata_offset1 =10 # 1 simxInt. Generated by the client or server. The amount of shift of the pure data buffer (applies to split chunks). 733 | simx_cmdheaderoffset_cmd=14 # 1 simxInt. Generated by the client (and used in a reply by the server). The command combined with the operation mode of the command. 734 | simx_cmdheaderoffset_delay_or_split =18 # 1 simxUShort. Generated by the client or server. The amount of delay in ms of a continuous command or the max. pure data size to send at once (applies to split commands). 735 | simx_cmdheaderoffset_sim_time =20 # 1 simxInt. Generated by the server. The simulation time (in ms) when the command was executed (or 0 if simulation is not running) 736 | simx_cmdheaderoffset_status =24 # 1 byte. Generated by the server. (1 bit 0 is set --> error in function execution on server side). The client writes bit 1 if command cannot be overwritten 737 | simx_cmdheaderoffset_reserved =25 # 1 byte. Not yet used 738 | 739 | 740 | 741 | 742 | 743 | # Regular operation modes 744 | simx_opmode_oneshot =0x000000 # sends command as one chunk. Reply will also come as one chunk. Doesn't wait for the reply. 745 | simx_opmode_blocking =0x010000 # sends command as one chunk. Reply will also come as one chunk. Waits for the reply (_REPLY_WAIT_TIMEOUT_IN_MS is the timeout). 746 | simx_opmode_oneshot_wait =0x010000 # sends command as one chunk. Reply will also come as one chunk. Waits for the reply (_REPLY_WAIT_TIMEOUT_IN_MS is the timeout). 747 | simx_opmode_continuous =0x020000 748 | simx_opmode_streaming =0x020000 # sends command as one chunk. Command will be stored on the server and always executed 749 | #(every x ms (as far as possible) where x can be 0-65535. just add x to opmode_continuous). 750 | # A reply will be sent continuously each time as one chunk. Doesn't wait for the reply. 751 | 752 | # Operation modes for heavy data 753 | simx_opmode_oneshot_split =0x030000 # sends command as several chunks (max chunk size is x bytes where x can be _MIN_SPLIT_AMOUNT_IN_BYTES-65535. Just add x to opmode_oneshot_split). Reply will also come as several chunks. Doesn't wait for the reply. 754 | simx_opmode_continuous_split =0x040000 755 | simx_opmode_streaming_split =0x040000 # sends command as several chunks (max chunk size is x bytes where x can be _MIN_SPLIT_AMOUNT_IN_BYTES-65535. Just add x to opmode_continuous_split). Command will be stored on the server and always executed. A reply will be sent continuously each time as several chunks. Doesn't wait for the reply. 756 | 757 | # Special operation modes 758 | simx_opmode_discontinue =0x050000 # removes and cancels all commands stored on the client or server side (also continuous commands) 759 | simx_opmode_buffer =0x060000 # doesn't send anything but checks if a reply for the given command is available in the input buffer (i.e. previously received from the server) 760 | simx_opmode_remove =0x070000 # doesn't send anything and doesn't return any specific value. It just erases a similar command reply in the inbox (to free some memory) 761 | 762 | 763 | # Command return codes 764 | simx_return_ok =0x000000 765 | simx_return_novalue_flag =0x000001 # input buffer doesn't contain the specified command 766 | simx_return_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode 767 | simx_return_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode 768 | simx_return_remote_error_flag =0x000008 # command caused an error on the server side 769 | simx_return_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) 770 | simx_return_local_error_flag =0x000020 # command caused an error on the client side 771 | simx_return_initialize_error_flag =0x000040 # simxStart was not yet called 772 | 773 | # Following for backward compatibility (same as above) 774 | simx_error_noerror =0x000000 775 | simx_error_novalue_flag =0x000001 # input buffer doesn't contain the specified command 776 | simx_error_timeout_flag =0x000002 # command reply not received in time for opmode_oneshot_wait operation mode 777 | simx_error_illegal_opmode_flag =0x000004 # command doesn't support the specified operation mode 778 | simx_error_remote_error_flag =0x000008 # command caused an error on the server side 779 | simx_error_split_progress_flag =0x000010 # previous similar command not yet fully processed (applies to opmode_oneshot_split operation modes) 780 | simx_error_local_error_flag =0x000020 # command caused an error on the client side 781 | simx_error_initialize_error_flag =0x000040 # simxStart was not yet called 782 | 783 | 784 | -------------------------------------------------------------------------------- /code/planner.py: -------------------------------------------------------------------------------- 1 | ''' 2 | 16-662 Robot Autonomy (Spring 2019) 3 | Homework 2 - Motion Planning and Collision Avoidance 4 | Author: Heethesh Vhavle 5 | Email: heethesh@cmu.edu 6 | Version: 1.0.0 7 | 8 | The order of the targets are as follows: 9 | joint_1 / revolute / arm_base_link <- shoulder_link 10 | joint_2 / revolute / shoulder_link <- elbow_link 11 | joint_3 / revolute / elbow_link <- forearm_link 12 | joint_4 / revolute / forearm_link <- wrist_link 13 | joint_5 / revolute / wrist_link <- gripper_link 14 | joint_6 / prismatic / gripper_link <- finger_r 15 | joint_7 / prismatic / gripper_link <- finger_l 16 | ''' 17 | 18 | # Python 2/3 compatibility 19 | from __future__ import print_function 20 | 21 | import os 22 | import sys 23 | import time 24 | import pickle 25 | import pprint 26 | import traceback 27 | 28 | import numpy as np 29 | import matplotlib.pyplot as plt 30 | from sklearn.neighbors import KDTree 31 | from mpl_toolkits.mplot3d import Axes3D 32 | from mpl_toolkits.mplot3d.art3d import Poly3DCollection, Line3DCollection 33 | 34 | import utils 35 | import locobot_joint_ctrl 36 | from controller import ArmController 37 | from utilities import vrep_utils as vu 38 | from cuboid_collision import CollisionChecker 39 | from forward_kinematics import ForwardKinematicsSolver 40 | 41 | # Pretty printer 42 | pp = pprint.PrettyPrinter(indent=4) 43 | 44 | 45 | class ProbabilisticRoadMap: 46 | def __init__(self, clientID, joint_targets, gripper_targets, urdf, 47 | save_graph=True, load_graph=False): 48 | 49 | # VREP client ID 50 | self.clientID = clientID 51 | 52 | # Targets 53 | self.joint_targets = { 'start': joint_targets[0], 'end': joint_targets[1] } 54 | self.gripper_targets = { 'start': gripper_targets[0], 'end': gripper_targets[1] } 55 | 56 | # PRM variables 57 | self.roadmap = {} 58 | self.kdtree = None 59 | self.save_graph = save_graph 60 | self.load_graph = load_graph 61 | 62 | # SAT collision checker 63 | self.collision_checker = CollisionChecker() 64 | 65 | # Get collision cuboids 66 | self.link_cuboids = utils.get_cuboids(self.clientID, utils.LINK_NAMES) 67 | self.obstacle_cuboids = utils.get_cuboids(self.clientID, utils.OBSTACLE_NAMES, append=False) 68 | 69 | # URDF parser 70 | self.robot = utils.URDFRobot(urdf) 71 | 72 | # Forward kinematics solver 73 | self.fksolver = ForwardKinematicsSolver(self.robot) 74 | 75 | # Initialize 76 | self.setup() 77 | 78 | def setup(self): 79 | # Get initial joint positions and link cuboids from VREP 80 | joint_angles = vu.get_arm_joint_positions(self.clientID) 81 | joint_poses = utils.get_joint_pose_vrep(self.clientID) 82 | self.fksolver.compute(self.link_cuboids, joint_angles, joint_poses, setup=True) 83 | 84 | # Simulation no longer required for planning 85 | vu.stop_sim(self.clientID) 86 | 87 | def plan(self, N=10, K=3, verbose=False): 88 | # Create the roadmap graph 89 | if self.load_graph: 90 | print('\nLoading roadmap graph and KD tree...') 91 | with open(r'graph.pickle', 'rb') as file: 92 | self.kdtree, self.roadmap = pickle.load(file) 93 | print('Done') 94 | else: self.construct_roadmap(N=N, K=K, verbose=verbose) 95 | 96 | # Query the roadmap and plan a path 97 | if self.query_roadmap(K=K, verbose=verbose): 98 | joint_plan = [ self.kdtree.data[waypoint] for waypoint in self.path ] 99 | joint_plan = np.asarray([ self.joint_targets['start'] ] + joint_plan + [ self.joint_targets['end'] ]) 100 | gripper_plan = np.linspace(self.gripper_targets['start'], self.gripper_targets['end'], num=len(self.path) + 2) 101 | 102 | # Return the trajectory 103 | print('\nJoint plan:') 104 | print(joint_plan) 105 | return True, joint_plan, gripper_plan 106 | 107 | # Failed to plan a trajectory 108 | else: 109 | return False, None, None 110 | 111 | ############### Roadmap construction ############### 112 | 113 | def construct_roadmap(self, N, K, verbose=False): 114 | # Random sampling 115 | print('\nRandom sampling...') 116 | nodes = [] 117 | while len(nodes) < N: 118 | sample = self.get_random_sample() 119 | 120 | # Check is sampled point is in collision and above ground 121 | if self.collision_free(sample, verbose=verbose): 122 | nodes.append(sample) 123 | 124 | # Create a KDTree 125 | nodes = np.asarray(nodes) 126 | self.kdtree = KDTree(nodes) 127 | 128 | # Construct roadmap graph 129 | print('\nConstructing roadmap graph...') 130 | for i, node in enumerate(nodes): 131 | # Find k-nearest neighbors 132 | [distances], [args] = self.kdtree.query(node.reshape(1, -1), k=K + 1) 133 | if verbose: print('%d NN:' % K, i, args[1:]) 134 | 135 | # Run local planner between current node and all nearest nodes 136 | for arg, dist in zip(args[1:], distances[1:]): 137 | if self.local_planner(node, nodes[arg]): 138 | self.roadmap.setdefault(i, {})[arg] = dist 139 | 140 | # Save KDTree and roadmap 141 | if self.save_graph: 142 | print('\nSaving roadmap graph and KD tree...') 143 | with open(r'graph.pickle', 'wb') as file: 144 | pickle.dump([self.kdtree, self.roadmap], file) 145 | 146 | print('Done') 147 | 148 | def get_random_sample(self): 149 | limits = [ self.robot.get_joint_limits(j) for j in utils.JOINT_NAMES[:5] ] 150 | joints = [ np.random.uniform(i, j) for (i, j) in limits ] 151 | return np.asarray(joints) 152 | 153 | def collision_free(self, joint_angles=None, verbose=False): 154 | # Compute link poses using forward kinematics 155 | if joint_angles is not None: 156 | R, T, self.link_cuboids = self.fksolver.compute(self.link_cuboids, joint_angles) 157 | 158 | # Collision check between obstacles and arm 159 | for link in self.link_cuboids[1:]: 160 | for obstacle in self.obstacle_cuboids: 161 | if self.collision_checker.detect_collision_optimized(link, obstacle): 162 | if verbose: print('Collision between "%s" and "%s"' % (link.name, obstacle.name)) 163 | return False 164 | 165 | # Self-collision check 166 | for i, link_a in enumerate(self.link_cuboids[1:]): 167 | for j, link_b in enumerate(self.link_cuboids[1:]): 168 | # Skip self-link, adjacent links check, and gripper-fingers check 169 | if (i == j) or (i == j + 1) or (i == j - 1) \ 170 | or (i == 4 and (j in [5, 6])) or (j == 4 and (i in [5, 6])): continue 171 | elif self.collision_checker.detect_collision_optimized(link_a, link_b): 172 | if verbose: print('Self-collision between "%s" and "%s"' % (link_a.name, link_b.name)) 173 | return False 174 | 175 | # Check if all link vertices are above ground plane 176 | for link in self.link_cuboids[1:]: 177 | if np.sum(link.vertices[:, -1].flatten() < 0): 178 | if verbose: print('Link "%s" is below ground plane' % link.name) 179 | return False 180 | 181 | # All checks passed 182 | return True 183 | 184 | def local_planner(self, start, end, n_points=5, verbose=False): 185 | # Check dimensions 186 | assert len(start) == len(end) 187 | 188 | # Interpolate to form a local path 189 | local_path = np.linspace(start, end, num=n_points) 190 | 191 | # Check for collisions along the path 192 | for point in local_path: 193 | if not self.collision_free(point, verbose=verbose): 194 | return False 195 | 196 | # Path planning successful 197 | return True 198 | 199 | ############### Roadmap query ############### 200 | 201 | def query_roadmap(self, K=3, verbose=False): 202 | # Add start and end targets to the roadmap 203 | start = self.joint_targets['start'] 204 | end = self.joint_targets['end'] 205 | self.add_targets_to_roadmap(start, end, K=K, verbose=verbose) 206 | self.roadmap = utils.convert_directed_to_undirected_graph(self.roadmap) 207 | 208 | # Print roadmap 209 | if verbose: 210 | print('\nRoadmap:') 211 | pp.pprint(self.roadmap) 212 | self.display_roadmap() 213 | 214 | # Run the path planner 215 | return self.path_planner(verbose=verbose) 216 | 217 | def add_targets_to_roadmap(self, start, end, K, verbose=False): 218 | print('\nAdding targets to roadmap...') 219 | 220 | # Pick k-nearest neighbors for start and end nodes 221 | [start_dists], [start_args] = self.kdtree.query(start.reshape(1, -1), k=K) 222 | [end_dists], [end_args] = self.kdtree.query(end.reshape(1, -1), k=K) 223 | 224 | # Display target KNNs 225 | if verbose: 226 | print('%d NN: START' % K, start_args[1:]) 227 | print('%d NN: END' % K, end_args[1:]) 228 | 229 | # Connect start node to knn nodes in the roadmap 230 | for dist, arg in zip(start_dists, start_args): 231 | if self.local_planner(start, self.kdtree.data[arg]): 232 | self.roadmap.setdefault('start', {})[arg] = dist 233 | 234 | # Connect end node to knn nodes in the roadmap 235 | for dist, arg in zip(end_dists, end_args): 236 | if self.local_planner(end, self.kdtree.data[arg]): 237 | self.roadmap.setdefault('end', {})[arg] = dist 238 | 239 | print('Done') 240 | 241 | def path_planner(self, verbose=False): 242 | # Initialize planner 243 | path = [] 244 | parents = {} 245 | unvisited = self.roadmap 246 | 247 | # Initialize weights 248 | weights = { node: np.Inf for node in unvisited } 249 | weights['start'] = 0 250 | weights['end'] = np.Inf 251 | 252 | # Dijkstra's path planner 253 | print('\nRunning global planner...') 254 | while unvisited: 255 | # Find the nearest node (based on norm from KDTree) 256 | nearest = None 257 | for node in unvisited: 258 | if nearest is None or (nearest in self.roadmap and weights[node] < weights[nearest]): 259 | nearest = node 260 | 261 | # Explore children nodes 262 | exists = nearest in self.roadmap 263 | for child, weight in self.roadmap[nearest].items(): 264 | if exists and weight + weights[nearest] < weights[child]: 265 | # Update weights 266 | weights[child] = weight + weights[nearest] 267 | # Keep track of parent node 268 | parents[child] = nearest 269 | 270 | # Mark the node visited 271 | unvisited.pop(nearest) 272 | 273 | if verbose: 274 | print('\nWeights:') 275 | pp.pprint(weights) 276 | print('\nParents:') 277 | pp.pprint(parents) 278 | 279 | # Backtrack to start node 280 | cur_node = 'end' 281 | while True: 282 | # Reached start node 283 | if cur_node == 'start': 284 | path.reverse() 285 | self.path = path[:-1] 286 | break 287 | 288 | # Backtrack to parent node 289 | path.append(cur_node) 290 | try: cur_node = parents[cur_node] 291 | except KeyError: 292 | print('Failed to find a path! Try making the graph more dense.') 293 | return False 294 | 295 | print('Done') 296 | return True 297 | 298 | ############### Visualizations ############### 299 | 300 | def display_cuboids(self, cuboids=None, obstacles=False, return_ax=False): 301 | # Display collision cuboids 302 | if cuboids: 303 | return self.collision_checker.display_cuboids(cuboids, return_ax=return_ax) 304 | elif obstacles: 305 | return self.collision_checker.display_cuboids(self.link_cuboids + self.obstacle_cuboids, return_ax=return_ax) 306 | else: 307 | return self.collision_checker.display_cuboids(self.link_cuboids, return_ax=return_ax) 308 | 309 | def display_roadmap(self, title=None, savefile=None): 310 | # Get cuboid display figure object 311 | ax = self.display_cuboids(self.obstacle_cuboids, return_ax=True) 312 | 313 | # Visualize the nodes in the roadmap 314 | for key in self.roadmap: 315 | # Get Cartesian space coordinates of root node 316 | if type(key) is int: 317 | try: key_pos = self.fksolver.compute(self.link_cuboids, self.kdtree.data[key])[1] 318 | except TypeError: 319 | print('Skipping display of %s children' % key) 320 | continue 321 | else: key_pos = self.fksolver.compute(self.link_cuboids, self.joint_targets[key][:5])[1] 322 | 323 | # Get Cartesian space coordinates of child nodes 324 | values_pos = [] 325 | for val in self.roadmap[key]: 326 | try: values_pos.append(self.fksolver.compute(self.link_cuboids, self.kdtree.data[val])[1]) 327 | except TypeError: 328 | print('Skipping display of node %s\'s children' % key) 329 | continue 330 | 331 | # Label 3D coordinates 332 | ax.text(key_pos[0], key_pos[1], key_pos[2], key if type(key) is str else str(key)) 333 | 334 | # Display edges 335 | colors = plt.get_cmap('tab10') 336 | for val_pos in values_pos: 337 | color = colors(key % 10) if type(key) is int else 'r' 338 | marker = 'o' if type(key) is int else 'x' 339 | ax.plot([key_pos[0], val_pos[0]], [key_pos[1], val_pos[1]], [key_pos[2], val_pos[2]], 340 | marker=marker, color=color, linestyle='-') 341 | 342 | if title: plt.title(title) 343 | if savefile: fig.savefig('%s.jpg' % savefile, dpi=480, bbox_inches='tight') 344 | else: plt.show() 345 | 346 | 347 | def setup_simulator(): 348 | # Connect to V-REP 349 | print ('\nConnecting to V-REP...') 350 | clientID = vu.connect_to_vrep() 351 | print ('Connected.') 352 | 353 | # Reset simulation in case something was running 354 | vu.reset_sim(clientID) 355 | 356 | # Initialize control inputs 357 | vu.set_arm_joint_target_velocities(clientID, np.zeros(vu.N_ARM_JOINTS)) 358 | vu.set_arm_joint_forces(clientID, 50.*np.ones(vu.N_ARM_JOINTS)) 359 | vu.step_sim(clientID, 1) 360 | 361 | return clientID 362 | 363 | 364 | def main(): 365 | success = False 366 | try: 367 | # Setup and reset simulator 368 | clientID = setup_simulator() 369 | 370 | # Joint targets, radians for revolute joints and meters for prismatic joints 371 | joint_targets = np.radians([[-80, 0, 0, 0, 0], [0, 60, -75, -75, 0]]) 372 | gripper_targets = np.asarray([[-0.03, 0.03], [-0.03, 0.03]]) 373 | 374 | # Initiate PRM planner 375 | prm_planner = ProbabilisticRoadMap(clientID, joint_targets, gripper_targets, 376 | urdf='urdf/locobot_description_v3.urdf', load_graph=True) 377 | 378 | # Run planner 379 | ret, joint_plan, gripper_plan = prm_planner.plan(N=100, K=10, verbose=False) 380 | 381 | # Planning successful 382 | if ret: 383 | # Setup and reset simulator 384 | clientID = setup_simulator() 385 | 386 | # Instantiate controller and execute the planned trajectory 387 | controller = ArmController(clientID) 388 | controller.execute(joint_plan, gripper_plan) 389 | success = True 390 | 391 | except Exception as e: 392 | print(e) 393 | traceback.print_exc() 394 | 395 | finally: 396 | # Stop VREP simulation 397 | vu.stop_sim(clientID) 398 | 399 | # Plot time histories 400 | if success: controller.plot(savefile=True) 401 | 402 | 403 | if __name__ == '__main__': 404 | main() 405 | -------------------------------------------------------------------------------- /code/utilities/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/code/utilities/__init__.py -------------------------------------------------------------------------------- /code/utilities/vrep_utils.py: -------------------------------------------------------------------------------- 1 | ''' 2 | 16-662 Robot Autonomy (Spring 2019) 3 | Homework 2 - Motion Planning and Collision Avoidance 4 | Author: Heethesh Vhavle 5 | Email: heethesh@cmu.edu 6 | Version: 1.0.0 7 | ''' 8 | 9 | # Import system libraries 10 | import os 11 | import sys 12 | import time 13 | from sets import Set 14 | 15 | sys.path.append(os.getcwd()) 16 | from lib2 import vrep 17 | 18 | # Import application libraries 19 | import numpy as np 20 | 21 | ### Global variables ########################################################## 22 | 23 | STREAMING_HANDLES_JOINT_POSITION = Set([]) 24 | STREAMING_HANDLES_JOINT_FORCE = Set([]) 25 | 26 | ARM_JOINT_NAMES = ['joint_1', # revolute / arm_base_link <- shoulder_link 27 | 'joint_2', # revolute / shoulder_link <- elbow_link 28 | 'joint_3', # revolute / elbow_link <- forearm_link 29 | 'joint_4', # revolute / forearm_link <- wrist_link 30 | 'joint_5', # revolute / wrist_link <- gripper_link 31 | 'joint_6', # prismatic / gripper_link <- finger_r 32 | 'joint_7'] # prismatic / gripper_link <- finger_l 33 | 34 | N_ARM_JOINTS = len(ARM_JOINT_NAMES) 35 | ARM_JOINT_HANDLES = None 36 | 37 | ### Utilities ################################################################# 38 | 39 | def connect_to_vrep(): 40 | vrep.simxFinish(-1) # just in case, close all opened connections 41 | clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5) # Connect to V-REP 42 | if clientID == -1: 43 | print('Failed connecting to V-REP remote API server.') 44 | exit() 45 | else: 46 | vrep.simxSynchronous(clientID, True) 47 | return clientID 48 | 49 | def step_sim(clientID, nrSteps=1): 50 | for _ in range(nrSteps): 51 | vrep.simxSynchronousTrigger(clientID) 52 | wait_for_sim(clientID) 53 | 54 | def wait_for_sim(clientID): 55 | vrep.simxGetPingTime(clientID) 56 | 57 | def stop_sim(clientID): 58 | vrep.simxStopSimulation(clientID, vrep.simx_opmode_oneshot) 59 | # TBD - change to polling V-REP server instead of sleeping 60 | time.sleep(2) 61 | 62 | def start_sim(clientID): 63 | vrep.simxStartSimulation(clientID, vrep.simx_opmode_oneshot) 64 | 65 | def reset_sim(clientID): 66 | stop_sim(clientID) 67 | start_sim(clientID) 68 | 69 | def get_sim_status(clientID): 70 | response, info = vrep.simxGetInMessageInfo(clientID, 71 | vrep.simx_headeroffset_server_state) 72 | assert response != -1, 'Did not receive a valid response.' 73 | return info 74 | 75 | def set_sim_dt(clientID, dt_seconds): 76 | # This only works if the custom time has been selected in the GUI. 77 | vrep.simxSetFloatingParameter(clientID, vrep.sim_floatparam_simulation_time_step, 78 | dt_seconds, vrep.simx_opmode_oneshot) 79 | 80 | ### Get and Set Methods ####################################################### 81 | 82 | def get_sim_time_seconds(clientID): 83 | return vrep.simxGetLastCmdTime(clientID)/1.e3 84 | 85 | def get_handle_by_name(clientID, name): 86 | response, handle = vrep.simxGetObjectHandle(clientID, name, 87 | vrep.simx_opmode_blocking) 88 | assert response == 0, 'Expected return code of 0, but instead return code was {}'.format(response) 89 | return handle 90 | 91 | def get_joint_position(clientID, handle): 92 | is_request_initial = handle not in STREAMING_HANDLES_JOINT_POSITION 93 | mode = vrep.simx_opmode_streaming if is_request_initial else vrep.simx_opmode_buffer 94 | valid_response = False 95 | while not valid_response: 96 | response, position = vrep.simxGetJointPosition(clientID, handle, mode) 97 | valid_response = response == 0 98 | if is_request_initial: 99 | STREAMING_HANDLES_JOINT_POSITION.add(handle) 100 | return position 101 | 102 | def set_joint_position(clientID, handle, position): 103 | response = vrep.simxSetJointPosition(clientID, handle, position, 104 | vrep.simx_opmode_oneshot) 105 | 106 | def get_joint_force(clientID, handle): 107 | is_request_initial = handle not in STREAMING_HANDLES_JOINT_FORCE 108 | mode = vrep.simx_opmode_streaming if is_request_initial else vrep.simx_opmode_buffer 109 | valid_response = False 110 | while not valid_response: 111 | response, force = vrep.simxGetJointForce(clientID, handle, mode) 112 | valid_response = response == 0 113 | if is_request_initial: 114 | STREAMING_HANDLES_JOINT_FORCE.add(handle) 115 | return force 116 | 117 | def set_joint_force(clientID, handle, force): 118 | # This method does not always set the force in the way you expect. 119 | # It will depend on the control and dynamics mode of the joint. 120 | response = vrep.simxSetJointForce(clientID, handle, force, 121 | vrep.simx_opmode_oneshot) 122 | 123 | def set_joint_target_velocity(clientID, handle, target_velocity): 124 | response = vrep.simxSetJointTargetVelocity(clientID, handle, target_velocity, 125 | vrep.simx_opmode_oneshot) 126 | 127 | def get_object_position(clientID, handle, relative_to_handle=-1): 128 | '''Return the object position in reference to the relative handle.''' 129 | response, position = vrep.simxGetObjectPosition(clientID, 130 | handle, 131 | relative_to_handle, 132 | vrep.simx_opmode_blocking) 133 | if response != 0: 134 | print("Error: Cannot query position for handle {} with reference to {}". 135 | format(handle, relative_to_handle)) 136 | return position 137 | 138 | def get_object_orientation(clientID, handle, reference_handle=-1): 139 | '''Return the object orientation in reference to the relative handle.''' 140 | response, orientation = vrep.simxGetObjectOrientation(clientID, 141 | handle, 142 | reference_handle, 143 | vrep.simx_opmode_blocking) 144 | if response != 0: 145 | print("Error: Cannot query position for handle {} with reference to {}". 146 | format(handle, reference_handle)) 147 | return orientation 148 | 149 | def get_object_quaternion(clientID, handle, reference_handle=-1): 150 | '''Return the object quaternion in reference to the relative handle.''' 151 | response, quaternion = vrep.simxGetObjectQuaternion(clientID, 152 | handle, 153 | reference_handle, 154 | vrep.simx_opmode_blocking) 155 | if response != 0: 156 | print("Error: Cannot query position for handle {} with reference to {}". 157 | format(handle, reference_handle)) 158 | return quaternion 159 | 160 | def get_object_bounding_box(clientID, handle): 161 | '''Return the bounding box for the given object handle.''' 162 | position_to_param_id = { 163 | 'min_x': 15, 'min_y': 16, 'min_z': 17, 164 | 'max_x': 18, 'max_y': 19, 'max_z': 20 165 | } 166 | position_to_value = {} 167 | for pos in position_to_param_id.keys(): 168 | param_id = position_to_param_id[pos] 169 | response, value = vrep.simxGetObjectFloatParameter( 170 | clientID, handle, param_id, vrep.simx_opmode_blocking) 171 | if response != 0: 172 | print("Error {}: Cannot get value for param {}".format( 173 | response, pos)) 174 | position_to_value[pos] = value 175 | min_pos = (position_to_value['min_x'], 176 | position_to_value['min_y'], 177 | position_to_value['min_z']) 178 | max_pos = (position_to_value['max_x'], 179 | position_to_value['max_y'], 180 | position_to_value['max_z']) 181 | return min_pos, max_pos 182 | 183 | 184 | ### LocoBot Methods ########################################################### 185 | 186 | def get_arm_joint_handles(clientID): 187 | global ARM_JOINT_HANDLES 188 | if not ARM_JOINT_HANDLES: 189 | # Cache arm joint handles to avoid repeated handle requests 190 | ARM_JOINT_HANDLES = [get_handle_by_name(clientID, j) for j in ARM_JOINT_NAMES] 191 | return ARM_JOINT_HANDLES 192 | 193 | def get_arm_joint_positions(clientID): 194 | joint_handles = get_arm_joint_handles(clientID) 195 | joint_positions = [get_joint_position(clientID, j) for j in joint_handles] 196 | return joint_positions 197 | 198 | def set_arm_joint_positions(clientID, positions): 199 | joint_handles = get_arm_joint_handles(clientID) 200 | assert len(positions) == len(joint_handles), \ 201 | 'Expected joint positions to be length {}, but it was length {} instead.'.format(len(positions), len(joint_handles)) 202 | for j, p in zip(joint_handles, positions): 203 | set_joint_position(clientID, j, p) 204 | 205 | def get_arm_joint_forces(clientID): 206 | joint_handles = get_arm_joint_handles(clientID) 207 | joint_forces = [get_joint_force(clientID, j) for j in joint_handles] 208 | return joint_forces 209 | 210 | def set_arm_joint_forces(clientID, forces): 211 | joint_handles = get_arm_joint_handles(clientID) 212 | assert len(forces) == len(joint_handles), \ 213 | 'Expected joint forces to be length {}, but it was length {} instead.'.format(len(forces), len(joint_handles)) 214 | for j, f in zip(joint_handles, forces): 215 | set_joint_force(clientID, j, f) 216 | 217 | def set_arm_joint_target_velocities(clientID, target_velocities): 218 | joint_handles = get_arm_joint_handles(clientID) 219 | assert len(target_velocities) == len(joint_handles), \ 220 | 'Expected joint target velocities to be length {}, but it was length {} instead.'.format(len(target_velocities), len(joint_handles)) 221 | for j, v in zip(joint_handles, target_velocities): 222 | set_joint_target_velocity(clientID, j, v) 223 | -------------------------------------------------------------------------------- /code/utils.py: -------------------------------------------------------------------------------- 1 | ''' 2 | 16-662 Robot Autonomy (Spring 2019) 3 | Homework 2 - Motion Planning and Collision Avoidance 4 | Author: Heethesh Vhavle 5 | Email: heethesh@cmu.edu 6 | Version: 1.0.0 7 | ''' 8 | 9 | # Python 2/3 compatibility 10 | from __future__ import print_function 11 | 12 | import os 13 | import sys 14 | 15 | import numpy as np 16 | from urdf_parser_py.urdf import URDF 17 | from tf.transformations import euler_matrix, quaternion_matrix, rotation_matrix, translation_matrix, decompose_matrix 18 | 19 | from cuboid_collision import Cuboid 20 | from utilities import vrep_utils as vu 21 | 22 | # Paths 23 | PKG_PATH = os.path.dirname(os.path.dirname(os.path.realpath(__file__))) 24 | 25 | # Handle names 26 | LINK_NAMES = ['arm_base_link_joint', 'shoulder_link', 'elbow_link', 'forearm_link', 'wrist_link', 'gripper_link', 'finger_r', 'finger_l'] 27 | JOINT_NAMES = ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6', 'joint_7'] 28 | OBSTACLE_NAMES = ['cuboid_0', 'cuboid_1', 'cuboid_2', 'cuboid_3', 'cuboid_4', 'cuboid_5'] 29 | 30 | 31 | def block_printing(func): 32 | def func_wrapper(*args, **kwargs): 33 | # Block all printing to the console 34 | sys.stdout = open(os.devnull, 'w') 35 | sys.stderr = open(os.devnull, 'w') 36 | 37 | # Function call 38 | value = func(*args, **kwargs) 39 | 40 | # Enable all printing to the console 41 | sys.stdout = sys.__stdout__ 42 | sys.stderr = sys.__stderr__ 43 | 44 | return value 45 | return func_wrapper 46 | 47 | 48 | def print_pose(pose): 49 | _, _, R, T, _ = decompose_matrix(pose) 50 | print('R:', np.degrees(R), 'T:', T, '\n') 51 | return R, T 52 | 53 | 54 | def convert_directed_to_undirected_graph(graph): 55 | undirected_graph = graph.copy() 56 | for node in graph: 57 | for child, weight in graph[node].items(): 58 | if child not in graph or node not in graph[child]: 59 | undirected_graph.setdefault(child, {})[node] = weight 60 | return undirected_graph 61 | 62 | 63 | class URDFRobot: 64 | def __init__(self, filename): 65 | self.robot = self.parse_urdf(filename) 66 | 67 | @block_printing 68 | def parse_urdf(self, filename): 69 | return URDF.from_xml_file(os.path.join(PKG_PATH, filename)) 70 | 71 | def find_joint_urdf(self, name): 72 | for joint in self.robot.joints: 73 | if joint.name == name: 74 | return joint 75 | return None 76 | 77 | def get_joint_axis(self, name): 78 | joint = self.find_joint_urdf(name) 79 | return np.asarray(joint.axis) 80 | 81 | def get_joint_limits(self, name): 82 | joint = self.find_joint_urdf(name) 83 | return (joint.limit.lower, joint.limit.upper) 84 | 85 | def get_joint_pose(self, name): 86 | joint = self.find_joint_urdf(name) 87 | T = translation_matrix(joint.origin.xyz) 88 | r, p, y = joint.origin.rpy 89 | R = euler_matrix(r, p, y) 90 | axis = np.asarray(joint.axis) 91 | return R, T, axis 92 | 93 | 94 | def get_joint_pose_vrep(clientID, joint_names=None): 95 | if not joint_names: 96 | joint_names = JOINT_NAMES 97 | 98 | poses = [] 99 | for handle in [vu.get_handle_by_name(clientID, j) for j in joint_names]: 100 | # Query VREP to get the parameters 101 | origin = vu.get_object_position(clientID, handle) 102 | rotation = vu.get_object_orientation(clientID, handle) 103 | poses.append((origin, rotation)) 104 | 105 | return poses 106 | 107 | 108 | def get_cuboids(clientID, names, append=True): 109 | if append: handles = [(vu.get_handle_by_name(clientID, j + '_collision_cuboid'), j) for j in names] 110 | else: handles = [(vu.get_handle_by_name(clientID, j), j) for j in names] 111 | 112 | cuboids = [] 113 | for (handle, name) in handles: 114 | # Query VREP to get the parameters 115 | min_pos, max_pos = vu.get_object_bounding_box(clientID, handle) 116 | origin = vu.get_object_position(clientID, handle) 117 | rotation = vu.get_object_orientation(clientID, handle) 118 | 119 | # Cuboid parameters 120 | min_pos = np.asarray(min_pos) 121 | max_pos = np.asarray(max_pos) 122 | dxyz = np.abs(max_pos - min_pos) 123 | 124 | # Cuboid objects 125 | cuboids.append(Cuboid(origin, rotation, dxyz, name, vrep=True)) 126 | 127 | return cuboids 128 | 129 | if __name__ == '__main__': 130 | robot = URDFRobot('urdf/locobot_description_v3.urdf') 131 | print(robot.get_joint_limits('joint_1')) 132 | -------------------------------------------------------------------------------- /handout.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/handout.pdf -------------------------------------------------------------------------------- /plots/cuboids/Cuboid 1 Collision: False .jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/cuboids/Cuboid 1 Collision: False .jpg -------------------------------------------------------------------------------- /plots/cuboids/Cuboid 2 Collision: False .jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/cuboids/Cuboid 2 Collision: False .jpg -------------------------------------------------------------------------------- /plots/cuboids/Cuboid 3 Collision: True .jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/cuboids/Cuboid 3 Collision: True .jpg -------------------------------------------------------------------------------- /plots/cuboids/Cuboid 4 Collision: True .jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/cuboids/Cuboid 4 Collision: True .jpg -------------------------------------------------------------------------------- /plots/cuboids/Cuboid 5 Collision: True .jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/cuboids/Cuboid 5 Collision: True .jpg -------------------------------------------------------------------------------- /plots/cuboids/Cuboid 6 Collision: False .jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/cuboids/Cuboid 6 Collision: False .jpg -------------------------------------------------------------------------------- /plots/cuboids/Cuboid 7 Collision: True .jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/cuboids/Cuboid 7 Collision: True .jpg -------------------------------------------------------------------------------- /plots/cuboids/Cuboid 8 Collision: True .jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/cuboids/Cuboid 8 Collision: True .jpg -------------------------------------------------------------------------------- /plots/images/1000-nodes.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/images/1000-nodes.png -------------------------------------------------------------------------------- /plots/images/Figure_1-1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/images/Figure_1-1.png -------------------------------------------------------------------------------- /plots/images/Figure_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/images/Figure_1.png -------------------------------------------------------------------------------- /plots/images/collision.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/images/collision.png -------------------------------------------------------------------------------- /plots/images/roadmap-100-10-01-01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/images/roadmap-100-10-01-01.png -------------------------------------------------------------------------------- /plots/images/roadmap-100-10-01-02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/images/roadmap-100-10-01-02.png -------------------------------------------------------------------------------- /plots/images/roadmap-100-10-01-03.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/images/roadmap-100-10-01-03.png -------------------------------------------------------------------------------- /plots/images/roadmap-100-10-01-04.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/images/roadmap-100-10-01-04.png -------------------------------------------------------------------------------- /plots/images/roadmap.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/images/roadmap.png -------------------------------------------------------------------------------- /plots/joints/joint-1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/joints/joint-1.jpg -------------------------------------------------------------------------------- /plots/joints/joint-2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/joints/joint-2.jpg -------------------------------------------------------------------------------- /plots/joints/joint-3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/joints/joint-3.jpg -------------------------------------------------------------------------------- /plots/joints/joint-4.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/joints/joint-4.jpg -------------------------------------------------------------------------------- /plots/joints/joint-5.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/joints/joint-5.jpg -------------------------------------------------------------------------------- /plots/joints/joint-6.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/joints/joint-6.jpg -------------------------------------------------------------------------------- /plots/joints/joint-7.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/plots/joints/joint-7.jpg -------------------------------------------------------------------------------- /scene/locobot_motion.ttt: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/scene/locobot_motion.ttt -------------------------------------------------------------------------------- /urdf/locobot_description_v3.urdf: -------------------------------------------------------------------------------- 1 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | 150 | 151 | 152 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | 161 | 162 | 163 | 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | 173 | 174 | 175 | 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | 185 | 186 | 187 | 188 | 189 | 190 | 191 | 192 | 193 | 195 | 196 | 199 | 201 | 208 | 209 | 210 | 213 | 214 | 216 | 217 | 219 | 221 | 222 | 223 | 224 | 227 | 228 | 230 | 231 | 232 | 233 | 234 | 235 | 236 | 237 | 238 | 240 | 241 | 244 | 246 | 253 | 254 | 255 | 258 | 259 | 261 | 262 | 264 | 266 | 267 | 268 | 269 | 272 | 273 | 275 | 276 | 277 | 278 | 281 | 284 | 286 | 288 | 290 | 295 | 296 | 298 | 299 | 302 | 304 | 311 | 312 | 313 | 316 | 317 | 319 | 320 | 322 | 324 | 325 | 326 | 327 | 330 | 331 | 333 | 334 | 335 | 336 | 339 | 342 | 344 | 346 | 348 | 353 | 354 | 356 | 357 | 360 | 362 | 369 | 370 | 371 | 374 | 375 | 377 | 378 | 380 | 382 | 383 | 384 | 385 | 388 | 389 | 391 | 392 | 393 | 394 | 397 | 400 | 402 | 404 | 406 | 407 | 408 | 409 | 410 | 411 | 412 | 413 | 414 | 415 | 416 | 417 | 418 | 419 | 420 | 421 | 422 | 423 | 424 | 425 | 426 | 427 | 428 | 429 | 430 | 431 | 432 | 433 | 434 | 435 | 436 | 437 | 438 | 439 | 440 | 441 | 442 | 443 | 444 | 445 | 446 | 447 | 448 | 449 | 450 | 451 | 452 | 453 | 454 | 455 | 456 | 457 | 458 | 459 | 460 | 461 | 462 | 463 | 464 | 465 | 466 | 467 | 468 | 469 | 470 | 471 | 472 | 473 | 474 | 475 | 476 | 477 | 478 | 479 | 480 | 481 | 482 | 483 | 484 | 485 | 486 | 487 | 488 | 489 | 490 | 491 | 492 | 493 | 494 | 495 | 496 | 497 | 498 | 499 | 500 | 501 | 502 | 503 | 504 | 505 | 506 | 507 | 508 | 509 | 510 | 511 | 512 | 513 | 514 | 515 | 516 | 517 | 518 | 519 | 520 | 521 | 522 | 523 | 524 | 525 | 526 | 527 | 528 | 529 | 530 | 531 | 532 | 533 | 534 | 535 | 536 | 537 | 538 | 539 | 540 | 541 | 542 | 543 | 544 | 545 | 546 | 547 | 548 | 549 | 550 | 551 | 552 | 553 | 554 | 555 | 556 | 557 | 558 | 559 | 560 | 561 | 562 | 563 | 564 | 565 | 566 | 567 | 568 | 569 | 570 | 571 | 572 | 573 | 574 | 575 | 576 | 577 | 578 | 579 | 580 | 581 | 582 | 583 | 584 | 585 | 586 | 587 | 588 | 589 | 590 | 591 | 592 | 593 | 594 | 595 | 596 | 597 | 598 | 599 | 600 | 601 | 602 | 603 | 604 | 605 | 606 | 607 | 608 | 609 | 610 | 611 | 612 | 613 | 614 | 615 | 616 | 617 | 618 | 619 | 620 | 621 | 622 | 623 | 624 | 625 | 626 | 627 | 628 | 629 | 630 | 631 | 632 | 633 | 634 | 635 | 636 | 637 | 638 | 639 | 640 | 641 | 642 | 643 | 644 | 645 | 646 | 647 | 648 | 649 | 650 | 651 | 652 | 653 | 654 | 655 | 656 | 657 | 658 | 659 | 660 | 661 | 662 | 663 | 664 | 665 | 666 | 667 | 668 | 669 | 670 | / 671 | 672 | 673 | 674 | 675 | transmission_interface/SimpleTransmission 676 | 677 | hardware_interface/EffortJointInterface 678 | 679 | 680 | hardware_interface/EffortJointInterface 681 | 1 682 | 683 | 684 | 685 | transmission_interface/SimpleTransmission 686 | 687 | hardware_interface/EffortJointInterface 688 | 689 | 690 | hardware_interface/EffortJointInterface 691 | 1 692 | 693 | 694 | 695 | 696 | transmission_interface/SimpleTransmission 697 | 698 | hardware_interface/EffortJointInterface 699 | 700 | 701 | hardware_interface/EffortJointInterface 702 | 1 703 | 704 | 705 | 706 | transmission_interface/SimpleTransmission 707 | 708 | hardware_interface/EffortJointInterface 709 | 710 | 711 | hardware_interface/EffortJointInterface 712 | 1 713 | 714 | 715 | 716 | transmission_interface/SimpleTransmission 717 | 718 | hardware_interface/EffortJointInterface 719 | 720 | 721 | hardware_interface/EffortJointInterface 722 | 1 723 | 724 | 725 | 726 | transmission_interface/SimpleTransmission 727 | 728 | hardware_interface/EffortJointInterface 729 | 730 | 731 | hardware_interface/EffortJointInterface 732 | 1 733 | 734 | 735 | 736 | transmission_interface/SimpleTransmission 737 | 738 | hardware_interface/EffortJointInterface 739 | 740 | 741 | hardware_interface/EffortJointInterface 742 | 1 743 | 744 | 745 | 746 | transmission_interface/SimpleTransmission 747 | 748 | hardware_interface/EffortJointInterface 749 | 750 | 751 | hardware_interface/EffortJointInterface 752 | 1 753 | 754 | 755 | 756 | transmission_interface/SimpleTransmission 757 | 758 | hardware_interface/EffortJointInterface 759 | 760 | 761 | hardware_interface/EffortJointInterface 762 | 1 763 | 764 | 765 | 766 | 767 | 768 | 30.0 769 | 770 | 1.3962634 771 | 772 | 800 773 | 800 774 | R8G8B8 775 | 776 | 777 | 0.02 778 | 300 779 | 780 | 781 | gaussian 782 | 785 | 0.0 786 | 0.007 787 | 788 | 789 | 790 | true 791 | 0.0 792 | /arm_camera 793 | image_raw 794 | camera_info 795 | camera_link 796 | 0.07 797 | 0.0 798 | 0.0 799 | 0.0 800 | 0.0 801 | 0.0 802 | 803 | 804 | 805 | 806 | 807 | true 808 | 809 | 810 | 811 | 812 | 813 | true 814 | false 815 | 40 816 | right_wheel_joint 817 | left_wheel_joint 818 | 0.235 819 | 0.072 820 | 1.0 821 | 822 | create_controller/cmd_vel 823 | odom 824 | odom 825 | base_footprint 826 | 827 | 828 | true 829 | 100.0 830 | base_link 831 | create_controller/absolute_position 832 | 0 833 | map 834 | 0 0 0 835 | 0 0 0 836 | 837 | 838 | 839 | 840 | 1.0 841 | 1.0 842 | 1000000.0 843 | 100.0 844 | 0.001 845 | 1.0 846 | 847 | 848 | 1.0 849 | 1.0 850 | 1000000.0 851 | 100.0 852 | 0.001 853 | 1.0 854 | 855 | 856 | 0.0 857 | 0.0 858 | 1000000.0 859 | 100.0 860 | 0.001 861 | 1.0 862 | 863 | 864 | 0.0 865 | 0.0 866 | 1000000.0 867 | 100.0 868 | 0.001 869 | 1.0 870 | 871 | 872 | 873 | true 874 | 20.0 875 | 0 0 0 0 0 0 876 | false 877 | 878 | 879 | 880 | 1 881 | 1 882 | 0 883 | 0 884 | 885 | 886 | 887 | 0.0160 888 | 0.04 889 | 0.1 890 | 891 | 892 | 893 | 894 | 895 | 896 | true 897 | 20.0 898 | 0 0 0 0 0 0 899 | false 900 | 901 | 902 | 903 | 1 904 | 1 905 | 0 906 | 0 907 | 908 | 909 | 910 | 0.01 911 | 0.04 912 | 0.1 913 | 914 | 915 | 916 | 917 | 918 | 919 | true 920 | 20.0 921 | 0 0 0 0 0 0 922 | false 923 | 924 | 925 | 926 | 1 927 | 1 928 | 0 929 | 0 930 | 931 | 932 | 933 | 0.01 934 | 0.04 935 | 0.1 936 | 937 | 938 | 939 | 940 | 941 | 942 | true 943 | 20.0 944 | 0 0 0 0 0 0 945 | false 946 | 947 | 948 | 949 | 1 950 | 1 951 | 0 952 | 0 953 | 954 | 955 | 956 | 0.01 957 | 0.04 958 | 0.1 959 | 960 | 961 | 962 | 963 | 964 | 965 | true 966 | 20.0 967 | 0 0 0 0 0 0 968 | false 969 | 970 | 971 | 972 | 1 973 | 1 974 | 0 975 | 0 976 | 977 | 978 | 979 | 0.01 980 | 0.04 981 | 0.1 982 | 983 | 984 | 985 | 986 | 987 | 988 | true 989 | 30 990 | gyro_link 991 | create/imu/data 992 | create/imu/is_calibrated 993 | 2.89e-06 994 | 0 0 0 995 | 0 0 0 996 | 997 | 998 | 999 | -------------------------------------------------------------------------------- /writeup/hvhvalen.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/heethesh/Probabilistic-Roadmap-Planner/2043b5c6e4509c64fdb9502bc5b6680bc5b59d16/writeup/hvhvalen.pdf --------------------------------------------------------------------------------