├── 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 |
8 |
9 | ## Cuboid Collision Detection using Seperating Axis Theorem
10 | 
11 |
12 | ## Probabilistic Roadmap Planner
13 | #### Generated Roadmap with 100 Nodes
14 | 
15 |
16 | #### Generated Roadmap with 1000 Nodes
17 | 
18 |
19 | ## LocoBot Manipulator
20 | #### LocoBot Manipulator with Bounding Boxes
21 | 
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
--------------------------------------------------------------------------------