8000 Merge branch 'master' of https://github.com/petercorke/robotics-toolb… · hzyjerry/robotics-toolbox-python@906f0e0 · GitHub
[go: up one dir, main page]
Skip to content

Commit 906f0e0

Browse files
petercorkepetercorke
committed
2 parents fbb50c7 + d531939 commit 906f0e0

File tree

3 files changed

+34
-17
lines changed

3 files changed

+34
-17
lines changed

‎roboticstoolbox/backends/VPython/VPython.py

Lines changed: 14 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -19,7 +19,7 @@
1919
close_localhost_session = None
2020

2121
try:
22-
from roboticstoolbox.backends.VPython.canvas import GraphicsCanvas2D, GraphicsCanvas3D
22+
from roboticstoolbox.backends.VPython.canvas import GraphicsCanvas2D, GraphicsCanvas3D, UImode
2323
from roboticstoolbox.backends.VPython.graphicalrobot import GraphicalRobot
2424
from roboticstoolbox.backends.VPython.grid import GridType
2525
except ImportError:
@@ -171,16 +171,18 @@ def step(self, dt=None, id=None, q=None, fig_num=0):
171171
# If GraphicalRobot given
172172
if isinstance(id, GraphicalRobot):
173173
if self.canvases[fig_num].is_robot_in(id):
174-
poses = id.fkine(q)
175-
id.set_joint_poses(poses)
174+
id.fkine_and_set(q)
175+
if self.canvases[fig_num].current_mode() == UImode.TEACHPANEL:
176+
# Reload the joint sliders
177+
self.canvases[fig_num].teach_mode(teach=True)
176178

177179
# If DHRobot is given (or equivalent)
178180
else:
179-
grpahical_dh_robot = None
181+
graphical_dh_robot = None
180182
# If no ID given, and there are robots available
181183
if id is None and len(self.robots) > 0:
182184
# Obtain the first one
183-
grpahical_dh_robot = self.robots[0]
185+
graphical_dh_robot = self.robots[0]
184186
# If no ID, and no robots available
185187
elif id is None:
186188
print("No robot found")
@@ -191,16 +193,19 @@ def step(self, dt=None, id=None, q=None, fig_num=0):
191193
if self.robots[i].robot is id and \
192194
self.canvases[fig_num].is_robot_in_canvas(
193195
self.robots[i]):
194-
grpahical_dh_robot = self.robots[i]
196+
graphical_dh_robot = self.robots[i]
195197
break
196198

197199
# If no graphical equivalent found, return
198-
if grpahical_dh_robot is None:
200+
if graphical_dh_robot is None:
199201
print("No robot found")
200202
return
201203
# Set poses of graphical robot
202-
poses = grpahical_dh_robot.fkine(q)
203-
grpahical_dh_robot.set_joint_poses(poses)
204+
graphical_dh_robot.fkine_and_set(q)
205+
206+
if self.canvases[fig_num].current_mode() == UImode.TEACHPANEL:
207+
# Reload the joint sliders
208+
self.canvases[fig_num].teach_mode(teach=True)
204209

205210
if dt is not None:
206211
sleep(dt)

‎roboticstoolbox/backends/VPython/canvas.py

Lines changed: 12 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -222,8 +222,9 @@ def add_robot(self, robot):
222222
self.__reload_caption(new_list)
223223

224224
# Set it as selected
225-
self.__ui_controls.get('menu_robots').index = \
226-
len(self.__robots) - 1
225+
if self.__ui_mode == UImode.CANVASCONTROL:
226+
self.__ui_controls.get('menu_robots').index = \
227+
len(self.__robots) - 1
227228

228229
# Place camera based on robots effective radius * 1.25
229230
if robot.robot is not None:
@@ -285,6 +286,9 @@ def set_grid_mode(self, mode):
285286
"""
286287
self.__graphics_grid.set_mode(mode)
287288

289+
def current_mode(self):
290+
return self.__ui_mode
291+
288292
#######################################
289293
# UI Management
290294
#######################################
@@ -605,6 +609,11 @@ def __setup_joint_sliders(self):
605609
if len(self.__teachpanel) == 0:
606610
self.scene.append_to_caption("No robots available\n")
607611
return
612+
613+
# Update the robots to their current joint angles
614+
for joint_idx, joint in enumerate(self.__teachpanel[self.__selected_robot]):
615+
joint[self.__idx_theta] = self.__robots[self.__selected_robot].angles[joint_idx]
616+
608617
i = 0
609618
for joint in self.__teachpanel[self.__selected_robot]:
610619
if joint[self.__idx_qlim_min] == joint[self.__idx_qlim_max]:
@@ -774,10 +783,7 @@ def __joint_slider(self, s):
774783
angles.append(self.__teachpanel_sliders[idx].value)
775784

776785
# Run fkine
777-
poses = self.__robots[self.__selected_robot].fkine(angles)
778-
779-
# Update joints
780-
self.__robots[self.__selected_robot].set_joint_poses(poses)
786+
self.__robots[self.__selected_robot].fkine_and_set(angles)
781787

782788
for joint in self.__teachpanel[self.__selected_robot]:
783789
if joint[self.__idx_text] is None:

‎roboticstoolbox/backends/VPython/graphicalrobot.py

Lines changed: 8 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -789,7 +789,7 @@ def animate(self, frame_poses, fps):
789789
while t_stop - t_start < f:
790790
t_stop = perf_counter()
791791

792-
def fkine(self, joint_angles):
792+
def fkine_and_set(self, joint_angles):
793793
"""
794794
Call fkine for the robot. If it is based on a seriallink object,
795795
run it's fkine function.
@@ -799,7 +799,13 @@ def fkine(self, joint_angles):
799799
"""
800800
# If seriallink object, run it's fkine
801801
if self.robot is not None:
802-
return self.robot.fkine_all(joint_angles)
802+
poses = self.robot.fkine_all(joint_angles)
803+
if joint_angles is None:
804+
joint_angles = self.robot.q
805+
for a_idx in range(len(joint_angles)):
806+
# Ignore the base's angle (idx == 0)
807+
self.angles[a_idx+1] = joint_angles[a_idx]
808+
self.set_joint_poses(poses)
803809
# Else TODO
804810
else:
805811
pass

0 commit comments

Comments
 (0)
0