Commit a7921888 authored by Nicolas Mansard's avatar Nicolas Mansard Committed by Nicolas Mansard
Browse files

[TP2] Final review of TP2 after discussion with Guilhem.

parent d4cd627d
This diff is collapsed.
......@@ -68,7 +68,7 @@ def callback(q):
viz.applyConfiguration('world/%s_des' % colors[i], list(targets[i]) + [1, 0, 0, 0])
viz.display(q)
time.sleep(1e-1)
time.sleep(1e-2)
Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([0.5, 0.1, 0.2])) # x,y,z
......
......@@ -38,7 +38,7 @@ def callback(q):
viz.applyConfiguration('world/%s_des' % colors[i], list(targets[i]) + [1, 0, 0, 0])
viz.display(q)
time.sleep(1e-1)
time.sleep(1e-2)
Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([0.5, 0.1, 0.2])) # x,y,z
......
......@@ -15,7 +15,7 @@ def callback(q):
viz.applyConfiguration(ballID, target.tolist() + [0, 1, 0, 0])
viz.applyConfiguration(tipID, robot.placement(q, 6))
viz.display(q)
time.sleep(1e-1)
time.sleep(1e-2)
target = np.array([0.5, 0.1, 0.2]) # x,y,z
......
# Add a vizualisation for the tip of the arm.
tipID = "world/blue"
viz.addBox(tipID, [.08] * 3, [.2, .2, 1., .5])
# Add a vizualisation for the tip of the arm.
tipID = "world/blue"
viz.addBox(tipID, [.08] * 3, [.2, .2, 1., .5])
#
# OPTIM 6D #########################################################
#
def cost(q):
'''Compute score from a configuration'''
M = robot.placement(q, 6)
......@@ -5,10 +14,10 @@ def cost(q):
def callback(q):
viz.applyConfiguration('world/box', Mtarget)
viz.applyConfiguration('world/blue', robot.placement(q, 6))
viz.applyConfiguration(boxID, Mtarget)
viz.applyConfiguration(tipID, robot.placement(q, 6))
viz.display(q)
time.sleep(1e-1)
time.sleep(1e-2)
Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([-0.5, 0.1, 0.2])) # x,y,z
......
from utils.load_ur5_parallel import load_ur5_parallel
# Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y.
robot = load_ur5_parallel()
# Open the viewer
viz = MeshcatVisualizer(robot)
# Add a new object featuring the parallel robot tool plate.
[w, h, d] = [0.5, 0.5, 0.005]
color = [red, green, blue, transparency] = [1, 1, 0.78, .3]
viz.addBox('world/robot0/toolplate', [w, h, d], color)
Mtool = pin.SE3(pin.utils.rotate('z', 1.268), np.array([0, 0, .75]))
viz.applyConfiguration('world/robot0/toolplate', Mtool)
viz.display(robot.q0)
effector_indexes = [robot.model.getFrameId('tool0_#%d' % i) for i in range(4)]
robot.framePlacement(robot.q0, effector_indexes[0])
......@@ -17,4 +17,4 @@ for i in range(50):
# Display new configuration for robot and ball
viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0])
viz.display(q)
time.sleep(0.1)
time.sleep(1e-2)
......@@ -17,4 +17,4 @@ for i in range(100):
# Display new configuration for robot and box
viz.applyConfiguration(boxID, oMbox)
viz.display(q)
time.sleep(0.1)
time.sleep(1e-2)
......@@ -28,7 +28,7 @@ viz = MeshcatVisualizer(robot)
# Define an init config
robot.q0 = np.array([0, -3.14 / 2, 0, 0, 0, 0])
viz.display(robot.q0)
time.sleep(1)
time.sleep(.3)
print("Let's go to pdes.")
# --- Add ball to represent target
......@@ -53,7 +53,7 @@ def callback(q):
viz.applyConfiguration(ballID, target.tolist() + [0, 1, 0, 0])
viz.applyConfiguration(tipID, robot.placement(q, 6))
viz.display(q)
time.sleep(1e-1)
time.sleep(1e-2)
target = np.array([0.5, 0.1, 0.2]) # x,y,z
......
......@@ -29,13 +29,14 @@ viz = MeshcatVisualizer(robot)
# Define an init config
robot.q0 = np.array([0, -3.14 / 2, 0, 0, 0, 0])
viz.display(robot.q0)
time.sleep(1)
time.sleep(.3)
print("Let's go to pdes.")
# --- Add box to represent target
# Add a vizualization for the target
boxID = "world/box"
viz.addBox(boxID, [.05, .1, .2], [1., .2, .2, .5])
# %do_load 1
# Add a vizualisation for the tip of the arm.
tipID = "world/blue"
viz.addBox(tipID, [.08] * 3, [.2, .2, 1., .5])
......@@ -45,7 +46,6 @@ viz.addBox(tipID, [.08] * 3, [.2, .2, 1., .5])
#
# %do_load 1
def cost(q):
'''Compute score from a configuration'''
M = robot.placement(q, 6)
......@@ -56,7 +56,7 @@ def callback(q):
viz.applyConfiguration(boxID, Mtarget)
viz.applyConfiguration(tipID, robot.placement(q, 6))
viz.display(q)
time.sleep(1e-1)
time.sleep(1e-2)
Mtarget = pin.SE3(pin.utils.rotate('x', 3.14 / 4), np.array([-0.5, 0.1, 0.2])) # x,y,z
......
......@@ -5,19 +5,31 @@ No optimization, this file is just an example of how to load the models.
import numpy as np
import pinocchio as pin
from utils.load_ur5_parallel import load_ur5_parallel
from utils.meshcat_viewer_wrapper import MeshcatVisualizer
# %do_load 0
from utils.load_ur5_parallel import load_ur5_parallel
# Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y.
robot = load_ur5_parallel()
# Open the viewer
viz = MeshcatVisualizer(url='classical')
viz = MeshcatVisualizer(robot)
# %end_load
viz.display(robot.q0)
# %do_load 1
# Add a new object featuring the parallel robot tool plate.
[w, h, d] = [0.5, 0.5, 0.005]
color = [red, green, blue, transparency] = [1, 1, 0.78, .3]
viz.addBox('world/robot0/toolplate', [w, h, d], color)
Mtool = pin.SE3(pin.utils.rotate('z', 1.268), np.array([0, 0, .75]))
viz.applyConfiguration('world/robot0/toolplate', Mtool)
viz.display(robot.q0)
# %end_load
# The 4 effector placements are computed by:
# %do_load 2
effector_indexes = [robot.model.getFrameId('tool0_#%d' % i) for i in range(4)]
robot.framePlacement(robot.q0, effector_indexes[0])
# %end_load
......@@ -73,7 +73,7 @@ for i in range(50):
# Display new configuration for robot and ball
viz.applyConfiguration(ballID, o_ball.tolist() + [1, 0, 0, 0])
viz.display(q)
time.sleep(0.1)
time.sleep(1e-2)
# %end_load
#
......@@ -129,11 +129,11 @@ for i in range(100):
# Display new configuration for robot and box
viz.applyConfiguration(boxID, oMbox)
viz.display(q)
time.sleep(0.1)
time.sleep(1e-2)
# %end_load
##########################################################################
### This last part is to automatically validate the versions of this example.
class SimplePickAndPlaceTest(unittest.TestCase):
def test_oMbox_translation(self):
self.assertTrue((np.abs(oMbox.translation - np.array([0.58907366, -0.09457755, 0.2335223])) < 1e-5).all())
self.assertTrue((np.abs(oMbox.translation - np.array([ 0.22085156, -0.6436716 , 0.5632217 ])) < 1e-5).all())
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment