Commit 7ac8e708 authored by Jayant Khatkar's avatar Jayant Khatkar
Browse files

calibrating in simulation, cleanup

parent 7b2e985c
......@@ -12,7 +12,7 @@ Includes the following important components:
- pynput
- gcode2contour
#### How to Run
#### How to Run on Real Hardware
1. Import git submodules.
......@@ -34,6 +34,8 @@ cd ..
```bash
# Connect to the UR5e and launch a controller service
# Replace the ip of the robot if necessary
# (if using a different robot, you will also need your own calibration file)
roslaunch ur_robot_driver ur5e_bringup.launch robot_ip:=192.168.1.95 kinematics_config:=$(pwd)/calibration.yaml
# Launch MoveIt and connect it to the controller service
......@@ -48,3 +50,13 @@ roslaunch ur5_e_moveit_config moveit_rviz.launch config:=true
```bash
ipython -i controller.py
```
#### How to Run in Simulation
1. Run the simulated UR5e.
```bash
# Launch simulated ur5e with moveit and visualisation
# TODO: change topic names of simulated to be same as real
launch ur5_e_moveit_config ur5_e_moveit_planning_execution.launch
```
......@@ -6,14 +6,20 @@ from math import pi
import time
import warnings
from utils import *
import sys
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from trajectory_msgs.msg import (
JointTrajectoryPoint,
JointTrajectory
)
from sensor_msgs.msg import JointState
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
class CalibrationType(Enum):
......@@ -45,12 +51,15 @@ class Calibration:
if len(self.cb_parameters) == self.cb_t.value:
warnings.warn("all parameters are already set, ignoring this input")
return
return False
self.cb_parameters.append(param)
if len(self.cb_parameters) == self.cb_t.value:
print "Calibration Complete"
return False
return True
class controller:
......@@ -67,6 +76,24 @@ class controller:
self.kin = ur_kinematics("UR5e")
time.sleep(1)
# initialise MoveIt commander
moveit_commander.roscpp_initialize(sys.argv)
self.robot = moveit_commander.RobotCommander()
self.scene = moveit_commander.PlanningSceneInterface()
self.group = moveit_commander.MoveGroupCommander("manipulator")
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
# Potentially useful but not used right now
# self.planning_frame = group.get_planning_frame()
# self.eef_link = group.get_end_effector_link()
# self.group_names = robot.get_group_names()
print self.robot.get_current_state()
def exec_traj(self, traj):
"""
......@@ -132,28 +159,19 @@ class controller:
"""
move the endpoint of the arm from where it currenlty is
"""
# step 1 get current point angles
now = self.get_joints()
# step 1 get current pose
now = self.group.get_current_pose().pose
# step 2 forward kinemtics giving transformation matrix
T = self.kin.fk(list(now))
# step 2 shift pose
now.position.x += dx
now.position.y += dy
now.position.z += dz
# step 3 Shift transformation matrix in desired direction
T[0,3] += dx
T[1,3] += dy
T[2,3] += dz
# step 3 compute cartesian plan
(plan, frac) = self.group.compute_cartesian_path([now], 0.01, 0.0)
# step 4 do inverse kinematics
js = self.kin.ik(T)
# step 5 choose closest joint angles to current angle
j, max_dTheta = self.kin.get_closest(js, list(now))
# step 6 move to new angle
if max_dTheta < pi/10:
self.move_to(j)
else:
print "ERROR"
# step 4 execute plan
self.group.execute(plan)
def _internal_on_press(self, key):
......@@ -164,20 +182,20 @@ class controller:
"""
if key == keyboard.Key.up:
self._internal_move_endpoint( 0.005, 0, 0)
self._internal_move_endpoint( 0.1, 0, 0)
elif key == keyboard.Key.down:
self._internal_move_endpoint(-0.005, 0, 0)
self._internal_move_endpoint(-0.1, 0, 0)
elif key == keyboard.Key.left:
self._internal_move_endpoint(0, 0.005, 0)
self._internal_move_endpoint(0, 0.1, 0)
elif key == keyboard.Key.right:
self._internal_move_endpoint(0, -0.005, 0)
self._internal_move_endpoint(0, -0.1, 0)
elif key == keyboard.Key.page_up:
self._internal_move_endpoint(0, 0, 0.005)
self._internal_move_endpoint(0, 0, 0.1)
elif key == keyboard.Key.page_down:
self._internal_move_endpoint(0, 0, -0.005)
self._internal_move_endpoint(0, 0, -0.1)
elif key == keyboard.Key.enter:
# save current pose into calibaration
self._temp_calib.push_parameter(self.get_joints())
return self._temp_calib.push_parameter(self.get_joints())
def _internal_on_release(self, key):
......
import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('python_controller',
anonymous=True)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "manipulator"
group = moveit_commander.MoveGroupCommander(group_name)
display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)
planning_frame = group.get_planning_frame()
eef_link = group.get_end_effector_link()
group_names = robot.get_group_names()
print robot.get_current_state()
joint_goal = group.get_current_joint_values()
joint_goal[0] = joint_goal[0] + 0.05
joint_goal[1] = joint_goal[1] + 0.05
joint_goal[2] = joint_goal[2] + 0.05
joint_goal[3] = joint_goal[3] + 0.05
joint_goal[4] = joint_goal[4] + 0.05
joint_goal[5] = joint_goal[5] + 0.05
group.go(joint_goal, wait=True)
group.stop()
from openrave_utils import *
from ur_kinematics import ur_kinematics
from openravepy import *
from time import sleep
import gcode2contour as gc
from math import pi
import numpy as np
from numpy import array
from numpy import matrix as mat
#from numpy.linalg import inv
def world2printTransform(point, Tr):
'''
world2printTransform
- This will convert world coordinates to robot coordinates.
- Additionally, it will set the angle of approach to be pointing down at the table
NOTE: This works when robot transforms are lateral, but not for rotations
Inputs:
- point: [0.3, 0, 1.1]
- Transformation matrix of robot relative to environment: (numpy matrix)
Output:
- Transformation matrix (numpy matrix) which can be fed directly to IK solver
'''
point.append(1)
transformed_point = np.concatenate(
# (mat([[1,0,0],[0,-1,0],[0,0,-1],[0,0,0]]), Tr * mat(point).T),
(mat([[0.0,-0.999,0.001],[-1.0,0.0,0.0],[0.0,-0.001,-0.999],[0,0,0]]), mat(point).T),
axis=1
)
transformed_point[3,3] = 1.0
return transformed_point
def interp_pose(pose1, pose2, t1, t2, ti):
"""
Linear interpolation of a pose between two times
Inputs:
- pose1 (list)
- pose2 (list)
- t1 (time at pose1)
- t2 (time at pose2)
- ti (time to interpolate for)
Output:
- pose_i (list)
"""
temp = (ti-t1)/(t2-t1)
return [p1 + (p2-p1)*temp for p1,p2 in zip(pose1, pose2)]
def show_contour(contour, env, linewidth=3):
"""
Display contour line in visualiser
Input: contour object (gcode2contour)
"""
return env.drawlinestrip(
points = array(contour.pos),
linewidth = linewidth,
colors=array((0,0,1))
)
def show_failed_contour(contour, env, linewidth=3):
"""
Display contour line in visualiser
Input: contour object (gcode2contour)
"""
return env.drawlinestrip(
points = array(contour.pos),
linewidth = linewidth,
colors=array((1,0,0))
)
def show_completed_contour(contour, env, linewidth=3):
"""
Display contour line in visualiser
Input: contour object (gcode2contour)
"""
return env.drawlinestrip(
points = array(contour.pos),
linewidth = linewidth,
colors=array((0,1,0))
)
Markdown is supported
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