Skip to content

Commit

Permalink
Add basic force term for DMP obstacle avoidance
Browse files Browse the repository at this point in the history
  • Loading branch information
af-a committed Apr 30, 2020
1 parent 157d1e2 commit 80834f1
Showing 1 changed file with 54 additions and 1 deletion.
Original file line number Diff line number Diff line change
Expand Up @@ -3,11 +3,13 @@
from std_msgs.msg import Float32MultiArray
from geometry_msgs.msg import PoseStamped, TwistStamped, Vector3Stamped, Twist
from nav_msgs.msg import Path
from mas_perception_msgs.msg import Object
import tf
from ros_dmp.roll_dmp import RollDmp
import pydmps

import yaml
from mas_knowledge_base.domestic_kb_interface import DomesticKBInterface


class DMPExecutor(object):
Expand Down Expand Up @@ -44,6 +46,10 @@ def __init__(self, dmp_name, tau):
self.dmp_name = dmp_name
self.tau = tau
self.roll_dmp = RollDmp(self.dmp_name, self.time_step, self.base_link_frame_name)
self.obs_avoidance_beta = 20. / np.pi
self.obs_avoidance_gamma = 100.

self.kb_interface = DomesticKBInterface()

self.min_sigma_value = None
self.goal = None
Expand Down Expand Up @@ -135,6 +141,9 @@ def follow_path(self, initial_pos, goal):
path: numpy.ndarray -- a 2D array of points (each row represents a point)
'''

obstacle_positions = self.get_nearby_obstacle_positions()

rospy.loginfo('[move_arm/dmp/follow_path] Executing motion')
trans, _ = self.get_transform(self.odom_frame_name, self.palm_link_name, rospy.Time(0))
current_pos = np.array([trans[0], trans[1], trans[2]])
Expand Down Expand Up @@ -164,7 +173,7 @@ def follow_path(self, initial_pos, goal):
self.motion_completed = True
break

next_pose, _, _ = self.dmp.step(tau=self.tau)
next_pose, _, _ = self.dmp.step(tau=self.tau, external_force=get_force_term(obstacle_positions))
next_pos = next_pose[:3]
current_path = np.vstack((current_path, next_pos))

Expand Down Expand Up @@ -300,6 +309,18 @@ def sigma_values_cb(self, msg):
'''
self.min_sigma_value = min(msg.data)

def get_nearby_obstacle_positions(self):
'''Queries the KB interface for the list of objects within 1 meter,
and returns a list of their positions.
'''
object_positions = []
perceived_objects = self.kb_interface.get_objects_within_distance(self.base_link_frame_name, Object._type, 1.0)

for obj in perceived_objects:
object_positions.append(obj.pose.position)

return object_positions

def instantiate_dmp(self, initial_pose, goal_pose):
'''Instantiates a DMP object from learned weights, given
the initial and final poses.
Expand Down Expand Up @@ -328,3 +349,35 @@ def instantiate_dmp(self, initial_pose, goal_pose):
goal=goal_pose,
ay=None,
w=dmp_weights)

def get_force_term(self, obstacle_positions):
'''Given the 3D postions of obstacles, generates the force term required
to avoid the obstacles.
Based on:
Hoffmann, Heiko, et al. "Biologically-inspired dynamical systems for movement generation:
automatic real-time goal adaptation and obstacle avoidance."
2009 IEEE International Conference on Robotics and Automation. IEEE, 2009.
Keyword arguments:
obstacle_positions: list -- positions of obstacles; numpy.ndarrays of size (3,)
'''
current_pos = self.dmp.current_pos[:3]
current_vel = self.dmp.current_vel[:3]
force_term = np.zeros(3)

for obstacle in obstacle_positions:
if np.linalg.norm(current_vel) > 1e-5:
obj_vec = obstacle - current_pos

phi = obj_vec.dot(current_vel) / (np.linalg.norm(obj_vec) * np.linalg.norm(current_vel))
dphi = obs_avoidance_gamma * phi * np.exp(-obs_avoidance_beta * abs(phi))
R = rotation_matrix(np.pi/2., np.cross(obstacle - current_pos, current_vel))[0:-1, 0:-1]

force_value = np.nan_to_num(np.dot(R, current_vel) * dphi)

# If object is farther away than the goal, ignore it:
if np.linalg.norm(obj_vec) > np.linalg.norm(self.dmp.goal - current_pos):
force_value = 0

force_term += force_value
return force_term

0 comments on commit 80834f1

Please sign in to comment.