hofee/nbv_sim
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Switch to a server/client architecture

Browse Source
This commit is contained in:
Michel Breyer 2021-04-15 10:44:08 +02:00
parent 622665cbc9
commit 5f21b7779c
12 changed files with 194 additions and 491 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.1)
project(vgn)
project(active_grasp)
find_package(catkin REQUIRED)

29
controllers.py
View File

@ -1,29 +0,0 @@
import numpy as np
class CartesianPoseController:
def __init__(self, robot, model, x0, rate):
self.robot = robot
self.model = model
self.rate = rate
self.x_d = x0
self.kp = np.ones(6) * 4.0
def set_target(self, pose):
self.x_d = pose
def update(self):
q, _ = self.robot.get_state()
x = self.model.pose(q)
x_d = self.x_d
err = np.zeros(6)
err[:3] = x_d.translation - x.translation
err[3:] = (x_d.rotation * x.rotation.inv()).as_rotvec()
J = self.model.jacobian(q)
J_pinv = np.linalg.pinv(J)
cmd = np.dot(J_pinv, self.kp * err)
self.robot.set_desired_joint_velocities(cmd)

1
launch/panda_visualization.launch
View File

@ -1,7 +1,6 @@
<?xml version="1.0" ?>
<launch>
<param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
<node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find active_grasp)/launch/panda_visualization.rviz" />
</launch>

66
launch/panda_visualization.rviz
View File

@ -5,8 +5,6 @@ Panels:
Property Tree Widget:
Expanded:
- /Global Options1
- /RobotModel1/Status1
- /TF1/Frames1
Splitter Ratio: 0.5
Tree Height: 549
- Class: rviz/Selection
@ -130,10 +128,34 @@ Visualization Manager:
Value: true
Visual Enabled: true
- Class: rviz/TF
Enabled: false
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: false
panda_hand:
Value: true
panda_leftfinger:
Value: false
panda_link0:
Value: true
panda_link1:
Value: false
panda_link2:
Value: false
panda_link3:
Value: false
panda_link4:
Value: false
panda_link5:
Value: false
panda_link6:
Value: false
panda_link7:
Value: false
panda_link8:
Value: false
panda_rightfinger:
Value: false
Marker Alpha: 1
Marker Scale: 0.5
Name: TF
@ -141,17 +163,21 @@ Visualization Manager:
Show Axes: true
Show Names: true
Tree:
panda_link0:
panda_link1:
panda_link2:
panda_link3:
panda_link4:
panda_link5:
panda_link6:
panda_link7:
panda_link8:
panda_hand:
panda_leftfinger:
{}
panda_rightfinger:
{}
Update Interval: 0
Value: false
- Class: rviz/InteractiveMarkers
Enable Transparency: true
Enabled: true
Name: Target
Show Axes: false
Show Descriptions: true
Show Visual Aids: false
Update Topic: /target/update
Value: true
Enabled: true
Global Options:
@ -181,7 +207,7 @@ Visualization Manager:
Views:
Current:
Class: rviz/Orbit
Distance: 1.2702323198318481
Distance: 1.3619381189346313
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
@ -189,17 +215,17 @@ Visualization Manager:
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0.06870806962251663
Y: 0.16815021634101868
Z: 0.3779522478580475
X: 0.14782209694385529
Y: 0.10340728610754013
Z: 0.3605891168117523
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.38539746403694153
Pitch: 0.21979668736457825
Target Frame: <Fixed Frame>
Yaw: 5.683584213256836
Yaw: 5.333564758300781
Saved: ~
Window Geometry:
Displays:
@ -217,5 +243,5 @@ Window Geometry:
Views:
collapsed: true
Width: 1200
X: 1045
Y: 249
X: 659
Y: 27

38
model.py
View File

@ -1,38 +0,0 @@
import numpy as np
import kdl_parser_py.urdf as kdl_parser
import PyKDL as kdl
import utils
class Model(object):
def __init__(self, root_frame_id, tip_frame_id):
_, tree = kdl_parser.treeFromFile("assets/urdfs/panda/panda_arm_hand.urdf")
chain = tree.getChain(root_frame_id, tip_frame_id)
self.nr_joints = chain.getNrOfJoints()
self.fk_pos_solver = kdl.ChainFkSolverPos_recursive(chain)
self.fk_vel_solver = kdl.ChainFkSolverVel_recursive(chain)
self.jac_solver = kdl.ChainJntToJacSolver(chain)
return
def pose(self, q):
jnt_array = utils.to_kdl_jnt_array(q)
frame = kdl.Frame()
self.fk_pos_solver.JntToCart(jnt_array, frame)
return utils.Transform.from_kdl(frame)
def velocities(self, q, dq):
jnt_array_vel = kdl.JntArrayVel(
utils.to_kdl_jnt_array(q), utils.to_kdl_jnt_array(dq)
)
twist = kdl.FrameVel()
self.fk_vel_solver.JntToCart(jnt_array_vel, twist)
d = twist.deriv()
linear, angular = np.r_[d[0], d[1], d[2]], np.r_[d[3], d[4], d[5]]
return linear, angular
def jacobian(self, q):
jnt_array = utils.to_kdl_jnt_array(q)
J = kdl.Jacobian(self.nr_joints)
self.jac_solver.JntToJac(jnt_array, J)
return utils.kdl_to_mat(J)

46
policies.py Normal file
View File

@ -0,0 +1,46 @@
from geometry_msgs.msg import Pose
import numpy as np
import rospy
import scipy.interpolate
from robot_tools.ros import *
def get_policy(name):
if name == "fixed":
return FixedTrajectory()
class BasePolicy:
def __init__(self):
self.tf_tree = TransformTree()
self.target_pose_pub = rospy.Publisher("/target", Pose, queue_size=10)
rospy.sleep(1.0)
class FixedTrajectory(BasePolicy):
def __init__(self):
super().__init__()
self.duration = 4.0
self.radius = 0.1
self.m = scipy.interpolate.interp1d([0, self.duration], [np.pi, 3.0 * np.pi])
def start(self):
self.tic = rospy.Time.now()
timeout = rospy.Duration(0.1)
x0 = self.tf_tree.lookup("panda_link0", "panda_hand", self.tic, timeout)
self.origin = np.r_[x0.translation[0] + self.radius, x0.translation[1:]]
self.target = x0
def update(self):
elapsed_time = (rospy.Time.now() - self.tic).to_sec()
if elapsed_time > self.duration:
return True
t = self.m(elapsed_time)
self.target.translation = (
self.origin + np.r_[self.radius * np.cos(t), self.radius * np.sin(t), 0.0]
)
self.target_pose_pub.publish(to_pose_msg(self.target))
return False

3
requirements.txt
View File

@ -1,3 +0,0 @@
numpy
scipy
pybullet

161
robot_sim.py
View File

@ -1,161 +0,0 @@
import time
import numpy as np
import pybullet as p
import pybullet_data
from sensor_msgs.msg import JointState
from model import *
from utils import *
class PandaArm:
def __init__(self):
self.nr_dof = 7
self.named_joint_values = {"ready": [0.0, -0.79, 0.0, -2.356, 0.0, 1.57, 0.79]}
self.upper_limits = [-7] * self.nr_dof
self.lower_limits = [7] * self.nr_dof
self.ranges = [7] * self.nr_dof
def load(self, pose):
self.T_world_base = pose
self.uid = p.loadURDF(
"assets/urdfs/panda/panda_arm_hand.urdf",
basePosition=pose.translation,
baseOrientation=pose.rotation.as_quat(),
useFixedBase=True,
)
for i, q_i in enumerate(self.named_joint_values["ready"]):
p.resetJointState(self.uid, i, q_i)
def get_state(self):
joint_states = p.getJointStates(self.uid, range(p.getNumJoints(self.uid)))[:7]
positions = [state[0] for state in joint_states]
velocities = [state[1] for state in joint_states]
return positions, velocities
def set_desired_joint_positions(self, positions):
for i, position in enumerate(positions):
p.setJointMotorControl2(self.uid, i, p.POSITION_CONTROL, position)
def set_desired_joint_velocities(self, velocities):
for i, velocity in enumerate(velocities):
p.setJointMotorControl2(
self.uid, i, p.VELOCITY_CONTROL, targetVelocity=velocity
)
class PandaGripper:
def move(self, width):
for i in [9, 10]:
p.setJointMotorControl2(
self.uid,
i,
p.POSITION_CONTROL,
0.5 * width,
force=10,
)
def read(self):
return p.getJointState(self.uid, 9)[0] + p.getJointState(self.uid, 10)[0]
class Camera:
def __init__(self, uid, link_id, width, height, hfov, near, far):
self.uid = uid
self.link_id = link_id
f = width / (2.0 * np.tan(hfov / 2.0))
cx, cy = width / 2.0, height / 2.0
self.width = width
self.height = height
self.K = [f, 0.0, cx, 0.0, f, cy, 0.0, 0.0, 1.0]
self.near = near
self.far = far
self.proj_mat = p.computeProjectionMatrixFOV(
np.rad2deg(hfov), width / height, near, far
)
def update(self):
result = p.getLinkState(self.uid, self.link_id, computeForwardKinematics=1)
t_world_cam = result[4]
R_world_cam = Rotation.from_quat(result[5])
eye = t_world_cam
target = R_world_cam.apply(np.r_[0.0, 0.0, 1.0]) + t_world_cam
up = R_world_cam.apply(np.r_[0.0, -1.0, 0.0])
view_mat = p.computeViewMatrix(eye, target, up)
result = p.getCameraImage(
self.width,
self.height,
view_mat,
self.proj_mat,
renderer=p.ER_BULLET_HARDWARE_OPENGL,
)
color_img, z_buffer = result[2][:, :, :3], result[3]
color_img = color_img.astype(np.uint8)
depth_img = (
self.far * self.near / (self.far - (self.far - self.near) * z_buffer)
).astype(np.float32)
return color_img, depth_img
class SimPandaEnv(object):
def __init__(self, gui, publish_state=True):
self.gui = gui
self.rate = 240
p.connect(p.GUI if gui else p.DIRECT)
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.setTimeStep(1.0 / self.rate)
p.setGravity(0.0, 0.0, -9.8)
self.step_cnt = 0
p.loadURDF("plane.urdf")
p.loadURDF(
"table/table.urdf",
baseOrientation=Rotation.from_rotvec(np.array([0, 0, np.pi / 2])).as_quat(),
useFixedBase=True,
)
p.loadURDF("cube_small.urdf", [0.0, 0.0, 0.8])
self.arm = PandaArm()
self.arm.load(Transform(Rotation.identity(), np.r_[-0.8, 0.0, 0.4]))
self.gripper = PandaGripper()
self.gripper.uid = self.arm.uid
self.model = Model("panda_link0", "panda_ee")
self.camera = Camera(self.arm.uid, 12, 320, 240, 1.047, 0.2, 2.0)
if publish_state:
self.state_pub = rospy.Publisher("/joint_states", JointState, queue_size=10)
rospy.Timer(rospy.Duration(1.0 / 30), self._publish_state)
def forward(self, dt):
steps = int(dt * self.rate)
for _ in range(steps):
self._step()
def _step(self):
if self.step_cnt % int(self.rate / self.controller.rate) == 0:
self.controller.update()
p.stepSimulation()
time.sleep(1.0 / self.rate)
def _publish_state(self, event):
positions, velocities = self.arm.get_state()
width = self.gripper.read()
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = ["panda_joint{}".format(i) for i in range(1, 8)] + [
"panda_finger_joint1",
"panda_finger_joint2",
]
msg.position = np.r_[positions, 0.5 * width, 0.5 * width]
msg.velocity = velocities
self.state_pub.publish(msg)

31
run.py Normal file
View File

@ -0,0 +1,31 @@
import argparse
from geometry_msgs.msg import Pose
import numpy as np
import rospy
from std_srvs.srv import Trigger
from policies import get_policy
def main(args):
rospy.init_node("panda_grasp")
policy = get_policy(args.policy)
r = rospy.Rate(args.rate)
done = False
policy.start()
while not done:
done = policy.update()
r.sleep()
# TODO execute grasp
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--policy", type=str, choices=["fixed"])
parser.add_argument("--rate", type=int, default=10)
args = parser.parse_args()
main(args)

23
run_demo.py
View File

@ -1,23 +0,0 @@
import rospy
from controllers import *
from robot_sim import *
from utils import *
gui = True
rospy.init_node("demo")
env = SimPandaEnv(gui)
q, dq = env.arm.get_state()
x0 = env.model.pose(q)
env.controller = CartesianPoseController(env.arm, env.model, x0, 60)
marker = InteractiveMarkerWrapper("target", "panda_link0", x0)
while True:
env.controller.set_target(marker.pose)
env.camera.update()
env.forward(0.1)

69
sim.py Normal file
View File

@ -0,0 +1,69 @@
import argparse
import numpy as np
import rospy
from geometry_msgs.msg import Pose
from sensor_msgs.msg import JointState
from robot_tools.btsim import BtPandaEnv
from robot_tools.controllers import CartesianPoseController
from robot_tools.ros import *
CONTROLLER_UPDATE_RATE = 60
JOINT_STATE_PUBLISHER_RATE = 60
class BtSimNode:
def __init__(self, gui):
self.sim = BtPandaEnv(gui=gui, sleep=False)
self.controller = CartesianPoseController(self.sim.arm)
self.joint_state_pub = rospy.Publisher(
"/joint_states", JointState, queue_size=10
)
rospy.Subscriber("/target", Pose, self._target_pose_cb)
def run(self):
rate = rospy.Rate(self.sim.rate)
self.step_cnt = 0
while not rospy.is_shutdown():
self._handle_updates()
self.sim.step()
self.step_cnt = (self.step_cnt + 1) % self.sim.rate
rate.sleep()
def _handle_updates(self):
if self.step_cnt % int(self.sim.rate / CONTROLLER_UPDATE_RATE) == 0:
self.controller.update()
if self.step_cnt % int(self.sim.rate / JOINT_STATE_PUBLISHER_RATE) == 0:
self._publish_joint_state()
def _publish_joint_state(self):
q, dq = self.sim.arm.get_state()
width = self.sim.gripper.read()
msg = JointState()
msg.header.stamp = rospy.Time.now()
msg.name = ["panda_joint{}".format(i) for i in range(1, 8)] + [
"panda_finger_joint1",
"panda_finger_joint2",
]
msg.position = np.r_[q, 0.5 * width, 0.5 * width]
msg.velocity = dq
self.joint_state_pub.publish(msg)
def _target_pose_cb(self, msg):
self.controller.set_target(from_pose_msg(msg))
def main(args):
rospy.init_node("bt_sim")
server = BtSimNode(args.gui)
server.run()
if __name__ == "__main__":
parser = argparse.ArgumentParser()
parser.add_argument("--gui", type=str, default=True)
args = parser.parse_args()
main(args)

214
utils.py
View File

@ -1,214 +0,0 @@
import numpy as np
from scipy.spatial.transform import Rotation
import PyKDL as kdl
import geometry_msgs.msg
from interactive_markers.interactive_marker_server import *
from interactive_markers.menu_handler import *
import std_msgs.msg
from visualization_msgs.msg import *
class InteractiveMarkerWrapper(object):
def __init__(self, name, frame_id, x0):
self.pose = x0
server = InteractiveMarkerServer(topic_ns=name)
int_marker = InteractiveMarker()
int_marker.header.frame_id = frame_id
int_marker.name = name
int_marker.scale = 0.2
int_marker.pose = to_pose_msg(x0)
# Attach visible sphere
marker = Marker()
marker.type = Marker.SPHERE
marker.scale = to_vector3_msg([0.05, 0.05, 0.05])
marker.color = to_color_msg([0.0, 0.5, 0.5, 0.6])
ctrl = InteractiveMarkerControl()
ctrl.always_visible = True
ctrl.markers.append(marker)
int_marker.controls.append(ctrl)
# Attach rotation controls
ctrl = InteractiveMarkerControl()
ctrl.name = "rotate_x"
ctrl.orientation = to_quat_msg(Rotation.from_quat([1, 0, 0, 1]))
ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(ctrl)
ctrl = InteractiveMarkerControl()
ctrl.name = "rotate_y"
ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 1, 0, 1]))
ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(ctrl)
ctrl = InteractiveMarkerControl()
ctrl.name = "rotate_z"
ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 0, 1, 1]))
ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
int_marker.controls.append(ctrl)
# Attach translation controls
ctrl = InteractiveMarkerControl()
ctrl.name = "move_x"
ctrl.orientation = to_quat_msg(Rotation.from_quat([1, 0, 0, 1]))
ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(ctrl)
ctrl = InteractiveMarkerControl()
ctrl.name = "move_y"
ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 1, 0, 1]))
ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(ctrl)
ctrl = InteractiveMarkerControl()
ctrl.name = "move_z"
ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 0, 1, 1]))
ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
int_marker.controls.append(ctrl)
server.insert(int_marker, self.cb)
server.applyChanges()
def cb(self, feedback):
self.pose = from_pose_msg(feedback.pose)
class Transform(object):
def __init__(self, rotation, translation):
self.rotation = rotation
self.translation = np.asarray(translation, np.double)
def as_matrix(self):
return np.vstack(
(np.c_[self.rotation.as_matrix(), self.translation], [0.0, 0.0, 0.0, 1.0])
)
def to_list(self):
return np.r_[self.rotation.as_quat(), self.translation]
def __mul__(self, other):
rotation = self.rotation * other.rotation
translation = self.rotation.apply(other.translation) + self.translation
return self.__class__(rotation, translation)
def inverse(self):
rotation = self.rotation.inv()
translation = -rotation.apply(self.translation)
return self.__class__(rotation, translation)
@classmethod
def identity(cls):
rotation = Rotation.from_quat([0.0, 0.0, 0.0, 1.0])
translation = np.array([0.0, 0.0, 0.0])
return cls(rotation, translation)
@classmethod
def from_matrix(cls, m):
rotation = Rotation.from_matrix(m[:3, :3])
translation = m[:3, 3]
return cls(rotation, translation)
@classmethod
def from_list(cls, list):
rotation = Rotation.from_quat(list[:4])
translation = list[4:]
return cls(rotation, translation)
@classmethod
def from_kdl(cls, f):
rotation = Rotation.from_matrix(
np.array(
[
[f.M[0, 0], f.M[0, 1], f.M[0, 2]],
[f.M[1, 0], f.M[1, 1], f.M[1, 2]],
[f.M[2, 0], f.M[2, 1], f.M[2, 2]],
]
)
)
translation = np.r_[f.p[0], f.p[1], f.p[2]]
return cls(rotation, translation)
# KDL Conversions
def to_kdl_jnt_array(q):
jnt_array = kdl.JntArray(len(q))
for i, q_i in enumerate(q):
jnt_array[i] = q_i
return jnt_array
def kdl_to_mat(m):
mat = np.zeros((m.rows(), m.columns()))
for i in range(m.rows()):
for j in range(m.columns()):
mat[i, j] = m[i, j]
return mat
# ROS Conversions
def to_color_msg(color):
msg = std_msgs.msg.ColorRGBA()
msg.r = color[0]
msg.g = color[1]
msg.b = color[2]
msg.a = color[3]
return msg
def to_point_msg(point):
msg = geometry_msgs.msg.Point()
msg.x = point[0]
msg.y = point[1]
msg.z = point[2]
return msg
def from_point_msg(msg):
return np.r_[msg.x, msg.y, msg.z]
def to_vector3_msg(vector3):
msg = geometry_msgs.msg.Vector3()
msg.x = vector3[0]
msg.y = vector3[1]
msg.z = vector3[2]
return msg
def from_vector3_msg(msg):
return np.r_[msg.x, msg.y, msg.z]
def to_quat_msg(orientation):
quat = orientation.as_quat()
msg = geometry_msgs.msg.Quaternion()
msg.x = quat[0]
msg.y = quat[1]
msg.z = quat[2]
msg.w = quat[3]
return msg
def from_quat_msg(msg):
return Rotation.from_quat([msg.x, msg.y, msg.z, msg.w])
def to_pose_msg(transform):
msg = geometry_msgs.msg.Pose()
msg.position = to_point_msg(transform.translation)
msg.orientation = to_quat_msg(transform.rotation)
return msg
def from_pose_msg(msg):
position = from_point_msg(msg.position)
orientation = from_quat_msg(msg.orientation)
return Transform(orientation, position)